void FixedwingAttitudeControl::task_main() { /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* do not limit the attitude updates in order to minimize latency. * actuator outputs are still limited by the individual drivers * properly to not saturate IO or physical limitations */ parameters_update(); /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _att_sub; fds[1].events = POLLIN; _task_running = true; while (!_task_should_exit) { static int loop_counter = 0; /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around * the pitch axis compared to the neutral position of the vehicle in multicopter mode * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. * Additionally, in order to get the correct sign of the pitch, we need to multiply * the new x axis of the rotation matrix with -1 * * original: modified: * * Rxx Ryx Rzx -Rzx Ryx Rxx * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ math::Matrix<3, 3> R; //original rotation matrix math::Matrix<3, 3> R_adapted; //modified rotation matrix R.set(_att.R); R_adapted.set(_att.R); /* move z to x */ R_adapted(0, 0) = R(0, 2); R_adapted(1, 0) = R(1, 2); R_adapted(2, 0) = R(2, 2); /* move x to z */ R_adapted(0, 2) = R(0, 0); R_adapted(1, 2) = R(1, 0); R_adapted(2, 2) = R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); PX4_R(_att.R, 0, 0) = R_adapted(0, 0); PX4_R(_att.R, 0, 1) = R_adapted(0, 1); PX4_R(_att.R, 0, 2) = R_adapted(0, 2); PX4_R(_att.R, 1, 0) = R_adapted(1, 0); PX4_R(_att.R, 1, 1) = R_adapted(1, 1); PX4_R(_att.R, 1, 2) = R_adapted(1, 2); PX4_R(_att.R, 2, 0) = R_adapted(2, 0); PX4_R(_att.R, 2, 1) = R_adapted(2, 1); PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; } vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); vehicle_status_poll(); /* lock integrator until control is started */ bool lock_integrator; if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { lock_integrator = true; } /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* default flaps to center */ float flaps_control = 0.0f; /* map flaps by default to manual if valid */ if (isfinite(_manual.flaps)) { flaps_control = _manual.flaps; } /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ float airspeed; /* if airspeed is not updating, we assume the normal average speed */ if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we * want to do in flight and its the baseline a human pilot would choose. * * Forcing the scaling to this value allows reasonable handheld tests. */ float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if * - velocity control or position control is enabled (pos controller is running) * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else { /* * Scale down roll and pitch as the setpoints are radians * and a typical remote can only do around 45 degrees, the mapping is * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; /* * in manual mode no external source should / does emit attitude setpoints. * emit the manual setpoint here to allow attitude controller tuning * in attitude control mode. */ struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* If the aircraft is on ground reset the integrators */ if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; control_input.roll = _att.roll; control_input.pitch = _att.pitch; control_input.yaw = _att.yaw; control_input.roll_rate = _att.rollspeed; control_input.pitch_rate = _att.pitchspeed; control_input.yaw_rate = _att.yawspeed; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; control_input.acc_body_x = _accel.x; control_input.acc_body_y = _accel.y; control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," " _roll_ctrl.get_desired_rate() %.4f," " _pitch_ctrl.get_desired_rate() %.4f" " att_sp.roll_body %.4f", (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } } float yaw_u = _yaw_ctrl.control_bodyrate(control_input); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (isfinite(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ _rates_sp.roll = _roll_ctrl.get_desired_rate(); _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else if (_actuators_id) { _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub > 0) { /* publish the actuator controls*/ orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _task_running = false; _exit(0); }
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { int mavlink_fd; mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.2f; float epv_vision = 0.2f; float eph_mocap = 0.05f; float epv_mocap = 0.05f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; uint16_t vision_updates = 0; uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_mocap[3][1] = { { 0.0f }, // N (pos) { 0.0f }, // E (pos) { 0.0f }, // D (pos) }; float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; float sonar_prev = 0.0f; //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct home_position_s home; memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct att_pos_mocap_s mocap; memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); px4_pollfd_struct_t fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, }; /* wait for initial baro value */ bool wait_baro = true; thread_running = true; while (wait_baro && !thread_should_exit) { int ret = px4_poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { baro_timestamp = sensor.baro_timestamp[0]; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; warnx("baro offset: %d m", (int)baro_offset); mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } } } } } /* main loop */ px4_pollfd_struct_t fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, }; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { /* act on attitude updates */ /* vehicle attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; bool updated; /* parameter update */ orb_check(parameter_update_sub, &updated); if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ orb_check(actuator_sub, &updated); if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ orb_check(armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* sensor combined */ orb_check(sensor_combined_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } acc[2] += CONSTANTS_ONE_G; } else { memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } if (sensor.baro_timestamp[0] != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; if (fabsf(corr_sonar) > params.sonar_err) { /* correction is too large: spike or new ground level? */ if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { /* spike detected, ignore */ corr_sonar = 0.0f; sonar_valid = false; } else { /* new ground level */ surface_offset -= corr_sonar; surface_offset_rate = 0.0f; corr_sonar = 0.0f; corr_sonar_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; } } float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; //todo check direction of x und y axis flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; flow_m[2] = z_est[1]; /* velocity in NED */ float flow_v[2] = { 0.0f, 0.0f }; /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /* home position */ orb_check(home_position_sub, &updated); if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; double est_lat, est_lon; float est_alt; if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); } /* update reference */ map_projection_init(&ref, home.lat, home.lon); /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; local_pos.ref_lat = home.lat; local_pos.ref_lon = home.lon; local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; if (ref_inited) { /* reproject position estimate with new reference */ map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } ref_inited = true; } } /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ orb_check(vision_position_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); static float last_vision_x = 0.0f; static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } vision_valid = true; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } /* calculate correction for position */ corr_vision[0][0] = vision.x - x_est[0]; corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; static hrt_abstime last_vision_time = 0; float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; last_vision_time = vision.timestamp_boot; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; vision.vy = (vision.y - last_vision_y) / vision_dt; vision.vz = (vision.z - last_vision_z) / vision_dt; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; /* calculate correction for velocity */ corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; } else { /* assume zero motion */ corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[2][1] = 0.0f - z_est[1]; } vision_updates++; } } /* vehicle mocap position */ orb_check(att_pos_mocap_sub, &updated); if (updated) { orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); /* reset position estimate on first mocap update */ if (!mocap_valid) { x_est[0] = mocap.x; y_est[0] = mocap.y; z_est[0] = mocap.z; mocap_valid = true; warnx("MOCAP data valid"); mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); } /* calculate correction for position */ corr_mocap[0][0] = mocap.x - x_est[0]; corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[2][0] = mocap.z - z_est[0]; mocap_updates++; } /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); bool reset_est = false; /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { ref_init_start = t; } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; local_pos.ref_lat = lat; local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; } /* calculate index of estimated values in buffer */ int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; corr_gps[1][1] = 0.0f; corr_gps[2][1] = 0.0f; } /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { /* no GPS lock */ memset(corr_gps, 0, sizeof(corr_gps)); ref_init_start = 0; } gps_updates++; } } /* check for timeout on FLOW topic */ if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; sonar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); } /* check for sonar measurement timeout */ if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { eph *= 1.0f + dt; } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ if (sonar_valid) { surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; surface_offset -= corr_sonar * params.w_z_sonar * dt; } } float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; float w_mocap_p = params.w_mocap_p; /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; w_xy_gps_v *= params.w_gps_flow; } /* baro offset correction */ if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr; } /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += R_gps[j][i] * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for VISION (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_vision_xy) { accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; } if (use_vision_z) { accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_mocap) { accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); if (use_gps_z) { epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { epv = fminf(epv, epv_vision); inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } if (use_mocap) { epv = fminf(epv, epv_mocap); inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { memcpy(z_est_prev, z_est, sizeof(z_est)); } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ if (use_flow) { eph = fminf(eph, eph_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (use_vision_xy) { eph = fminf(eph, eph_vision); inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } if (use_mocap) { eph = fminf(eph, eph_mocap); inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } } else { /* gradually reset xy velocity estimates */ inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), (double)(flow_updates / updates_dt), (double)(vision_updates / updates_dt), (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; vision_updates = 0; mocap_updates = 0; } } if (t > pub_last + PUB_INTERVAL) { pub_last = t; /* push current estimate to buffer */ est_buf[buf_ptr][0][0] = x_est[0]; est_buf[buf_ptr][0][1] = x_est[1]; est_buf[buf_ptr][1][0] = y_est[0]; est_buf[buf_ptr][1][1] = y_est[1]; est_buf[buf_ptr][2][0] = z_est[0]; est_buf[buf_ptr][2][1] = z_est[1]; /* push current rotation matrix to buffer */ memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); buf_ptr++; if (buf_ptr >= EST_BUF_SIZE) { buf_ptr = 0; } /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; } local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; global_pos.yaw = local_pos.yaw; global_pos.eph = eph; global_pos.epv = epv; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } } } warnx("stopped"); mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; }
void Navigator::gps_position_update() { orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); }
void task_main(int argc, char *argv[]) { _is_running = true; if (uart_initialize(_device) < 0) { PX4_ERR("Failed to initialize UART."); return; } // Subscribe for orb topics _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); // Start disarmed _armed.armed = false; _armed.prearmed = false; // Set up poll topic px4_pollfd_struct_t fds[1]; fds[0].fd = _controls_sub; fds[0].events = POLLIN; /* Don't limit poll intervall for now, 250 Hz should be fine. */ //orb_set_interval(_controls_sub, 10); // Set up mixer if (initialize_mixer(MIXER_FILENAME) < 0) { PX4_ERR("Mixer initialization failed."); return; } pwm_limit_init(&_pwm_limit); // TODO XXX: this is needed otherwise we crash in the callback context. _rc_pub = orb_advertise(ORB_ID(input_rc), &_rc); // Main loop while (!_task_should_exit) { int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); /* Timed out, do a periodic check for _task_should_exit. */ if (pret == 0) { continue; } /* This is undesirable but not much we can do. */ if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); _outputs.timestamp = _controls.timestamp; /* do mixing */ _outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL); /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { _outputs.output[i] = NAN; } const uint16_t reverse_mask = 0; uint16_t disarmed_pwm[4]; uint16_t min_pwm[4]; uint16_t max_pwm[4]; for (unsigned int i = 0; i < 4; i++) { disarmed_pwm[i] = _pwm_disarmed; min_pwm[i] = _pwm_min; max_pwm[i] = _pwm_max; } uint16_t pwm[4]; // TODO FIXME: pre-armed seems broken pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); send_outputs_mavlink(pwm, 4); if (_outputs_pub != nullptr) { orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); } else { _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); } } bool updated; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); } } uart_deinitialize(); orb_unsubscribe(_controls_sub); orb_unsubscribe(_armed_sub); _is_running = false; }
void *multirotor_position_control_thread_main() { /* welcome user */ fprintf (stdout, "Multirotor position controller started\n"); fflush(stdout); int i; bool_t updated; bool_t reset_mission_sp = 0 /* false */; bool_t global_pos_sp_valid = 0 /* false */; bool_t reset_man_sp_z = 1 /* true */; bool_t reset_man_sp_xy = 1 /* true */; bool_t reset_int_z = 1 /* true */; bool_t reset_int_z_manual = 0 /* false */; bool_t reset_int_xy = 1 /* true */; bool_t was_armed = 0 /* false */; bool_t reset_auto_sp_xy = 1 /* true */; bool_t reset_auto_sp_z = 1 /* true */; bool_t reset_takeoff_sp = 1 /* true */; absolute_time t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float i_limit; /* use integral_limit_out = tilt_max / 2 */ float ref_alt = 0.0f; absolute_time ref_alt_t = 0; absolute_time local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; PID_t z_pos_pid; thrust_pid_t z_vel_pid; float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; /* structures */ struct parameter_update_s ps; struct vehicle_control_flags_s control_flags; memset(&control_flags, 0, sizeof(control_flags)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; memset(&global_pos_sp, 0, sizeof(global_pos_sp)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update)); if (param_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n"); exit(-1); } orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (control_flags_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); if (att_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n"); exit(-1); } orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n"); exit(-1); } orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); if (manual_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n"); exit(-1); } orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); if (local_pos_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n"); exit(-1); } orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); if (local_pos_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n"); exit(-1); } orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); if (global_pos_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n"); exit(-1); } /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint)); if (local_pos_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint)); if (global_vel_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); /* abort on a nonzero return value from the parameter init */ if (multirotor_position_control_params_init() != 0) { /* parameter setup went wrong, abort */ fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n"); exit(-1); } for (i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f, multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i, multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); while (!_shutdown_all_systems) { updated = orb_check (ORB_ID(parameter_update), param_sub); if (updated) { /* clear updated flag */ orb_copy(ORB_ID(parameter_update), param_sub, &ps); /* update multirotor_position_control_parameters */ multirotor_position_control_params_update(); for (i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f); if (multirotor_position_control_parameters.xy_vel_i > 0.0f) { i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f; } else { i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max); } pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f, multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i, multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min); } updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags); } updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub); if (updated) { orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); global_pos_sp_valid = 1 /* true */; reset_mission_sp = 1 /* true */; } absolute_time t = get_absolute_time(); float dt; if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; } else { dt = 0.0f; } if (control_flags.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ reset_man_sp_z = 1 /* true */; reset_man_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; reset_auto_sp_xy = 1 /* true */; reset_takeoff_sp = 1 /* true */; reset_int_z = 1 /* true */; reset_int_xy = 1 /* true */; } was_armed = control_flags.flag_armed; t_prev = t; if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f; float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f; float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; if (control_flags.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ if (local_pos.ref_timestamp != ref_alt_t) { if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.ref_alt - ref_alt; } ref_alt_t = local_pos.ref_timestamp; ref_alt = local_pos.ref_alt; // TODO also correct XY setpoint } /* reset setpoints to current position if needed */ if (control_flags.flag_control_altitude_enabled) { if (reset_man_sp_z) { reset_man_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; //mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } } } if (control_flags.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); //mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max; sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ reset_auto_sp_xy = !control_flags.flag_control_position_enabled; reset_auto_sp_z = !control_flags.flag_control_altitude_enabled; reset_takeoff_sp = 1 /* true */; /* force reprojection of global setpoint after manual mode */ reset_mission_sp = 1 /* true */; } else if (control_flags.flag_control_auto_enabled) { /* AUTO mode, use global setpoint */ if (control_flags.auto_state == navigation_state_auto_ready) { reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_takeoff) { if (reset_takeoff_sp) { reset_takeoff_sp = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap; att_sp.yaw_body = att.yaw; //mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } reset_auto_sp_xy = 0 /* false */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_rtl) { // TODO reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_mission) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { reset_mission_sp = 1 /* true */; local_ref_timestamp = local_pos.ref_timestamp; double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); //mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (reset_mission_sp) { reset_mission_sp = 0 /* false */; /* update global setpoint projection */ if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.latitude * 1e-7; double sp_lon = global_pos_sp.longitude * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } att_sp.yaw_body = global_pos_sp.yaw; //mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (reset_auto_sp_xy) { reset_auto_sp_xy = 0 /* false */; /* local position setpoint is invalid, * use current position as setpoint for loiter */ local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; } if (reset_auto_sp_z) { reset_auto_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; } //mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } if (control_flags.auto_state != navigation_state_auto_takeoff) { reset_takeoff_sp = 1 /* true */; } if (control_flags.auto_state != navigation_state_auto_mission) { reset_mission_sp = 1 /* true */; } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* reset setpoints after AUTO mode */ reset_man_sp_xy = 1 /* true */; reset_man_sp_z = 1 /* true */; } else { /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ if (local_pos_sp.z < 5.0f) { /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; //mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_man_sp_z = 1 /* true */; } else { /* in air: hold altitude */ if (reset_man_sp_z) { reset_man_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; //mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } reset_auto_sp_z = 0 /* false */; } if (control_flags.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; att_sp.yaw_body = att.yaw; //mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } reset_auto_sp_xy = 0 /* false */; } } /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); /* run position & altitude controllers, calculate velocity setpoint */ if (control_flags.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { reset_man_sp_z = 1 /* true */; global_vel_sp.vz = 0.0f; } if (control_flags.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / multirotor_position_control_parameters.xy_vel_max; if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { reset_man_sp_xy = 1 /* true */; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } //fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz); /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ if (control_flags.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = 0 /* false */; float i = multirotor_position_control_parameters.thr_min; if (reset_int_z_manual) { i = manual.thrust; if (i < multirotor_position_control_parameters.thr_min) { i = multirotor_position_control_parameters.thr_min; } else if (i > multirotor_position_control_parameters.thr_max) { i = multirotor_position_control_parameters.thr_max; } } thrust_pid_set_integral(&z_vel_pid, -i); //mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } else { /* reset thrust integral when altitude control enabled */ reset_int_z = 1 /* true */; } if (control_flags.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ if (reset_int_xy) { reset_int_xy = 0 /* false */; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); //mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); /* assuming that vertical component of thrust is g, * horizontal component = g * tan(alpha) */ float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); if (tilt > multirotor_position_control_parameters.tilt_max) { tilt = multirotor_position_control_parameters.tilt_max; } /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } else { reset_int_xy = 1 /* true */; } att_sp.timestamp = get_absolute_time(); //fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else { /* position controller disabled, reset setpoints */ reset_man_sp_z = 1 /* true */; reset_man_sp_xy = 1 /* true */; reset_int_z = 1 /* true */; reset_int_xy = 1 /* true */; reset_mission_sp = 1 /* true */; reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled; /* run at approximately 50 Hz */ usleep(20000); } /* * do unsubscriptions */ orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self()); orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self()); /* * do unadvertises */ orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self()); return 0; }
void MulticopterAttitudeControl::task_main() { warnx("started"); fflush(stdout); /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ struct pollfd fds[1]; fds[0].fd = _v_att_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); } else { _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = (_manual_control_sp.z + 1) / 2; /* reset yaw setpoint after ACRO */ _reset_yaw_sp = true; /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); } else { _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } } } } perf_end(_loop_perf); } warnx("exit"); _control_task = -1; _exit(0); }
int ardrone_interface_thread_main(int argc, char *argv[]) { thread_running = true; char *device = "/dev/ttyS1"; /* welcome user */ printf("[ardrone_interface] Control started, taking over motors\n"); /* File descriptors */ int gpios; char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n"; bool motor_test_mode = false; int test_motor = -1; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { motor_test_mode = true; } if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { if (i+1 < argc) { int motor = atoi(argv[i+1]); if (motor > 0 && motor < 5) { test_motor = motor; } else { thread_running = false; errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); } } else { thread_running = false; errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); } } if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set if (argc > i + 1) { device = argv[i + 1]; } else { thread_running = false; errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); } } } struct termios uart_config_original; if (motor_test_mode) { printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); } /* Led animation */ int counter = 0; int led_counter = 0; /* declare and safely initialize all structs */ struct vehicle_status_s state; memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ gpios = ar_multiplexing_init(); if (ardrone_write < 0) { fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); thread_running = false; exit(ERROR); } /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); thread_running = false; exit(ERROR); } ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); // XXX Re-done initialization to make sure it is accepted by the motors // XXX should be removed after more testing, but no harm /* close uarts */ close(ardrone_write); /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ gpios = ar_multiplexing_init(); if (ardrone_write < 0) { fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); thread_running = false; exit(ERROR); } /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); thread_running = false; exit(ERROR); } while (!thread_should_exit) { if (motor_test_mode) { /* set motors to idle speed */ if (test_motor > 0 && test_motor < 5) { int motors[4] = {0, 0, 0, 0}; motors[test_motor - 1] = 10; ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); } else { ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); } } else { /* MAIN OPERATION MODE */ /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* for now only spin if armed and immediately shut down * if in failsafe */ if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { /* Silently lock down motor speeds to zero */ ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); } } if (counter % 24 == 0) { if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); led_counter++; if (led_counter == 12) led_counter = 0; } /* run at approximately 200 Hz */ usleep(4500); counter++; } /* restore old UART config */ int termios_state; if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); } printf("[ardrone_interface] Restored original UART config, exiting..\n"); /* close uarts */ close(ardrone_write); ar_multiplexing_deinit(gpios); fflush(stdout); thread_running = false; return OK; }
int uORBTest::UnitTest::test_multi2() { test_note("Testing multi-topic 2 test (queue simulation)"); //test: first subscribe, then advertise _thread_should_exit = false; const int num_instances = 3; int orb_data_fd[num_instances]; int orb_data_next = 0; for (int i = 0; i < num_instances; ++i) { // PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time()); orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i); } char *const args[1] = { nullptr }; int pubsub_task = px4_task_spawn_cmd("uorb_test_multi", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry, args); if (pubsub_task < 0) { return test_fail("failed launching task"); } hrt_abstime last_time = 0; while (!_thread_should_exit) { bool updated = false; int orb_data_cur_fd = orb_data_fd[orb_data_next]; orb_check(orb_data_cur_fd, &updated); if (updated) { struct orb_test_medium msg; orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg); // Relax timing requirement for Darwin CI system #ifdef __PX4_DARWIN usleep(10000); #else usleep(1000); #endif if (last_time >= msg.time && last_time != 0) { return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time); } last_time = msg.time; // PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time); orb_data_next = (orb_data_next + 1) % num_instances; } } for (int i = 0; i < num_instances; ++i) { orb_unsubscribe(orb_data_fd[i]); } return test_note("PASS multi-topic 2 test (queue simulation)"); }
int uORBTest::UnitTest::test_multi_reversed() { test_note("try multi-topic support subscribing before publishing"); /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */ /* Subscribe first and advertise afterwards. */ int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); if (sfd2 < 0) { return test_fail("sub. id2: ret: %d", sfd2); } struct orb_test t {}, u {}; t.val = 0; int instance2; _pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX); int instance3; _pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN); test_note("advertised"); if (instance2 != 2) { return test_fail("mult. id2: %d", instance2); } if (instance3 != 3) { return test_fail("mult. id3: %d", instance3); } t.val = 204; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) { return test_fail("mult. pub0 fail"); } t.val = 304; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) { return test_fail("mult. pub1 fail"); } test_note("published"); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) { return test_fail("sub #2 copy failed: %d", errno); } if (u.val != 204) { return test_fail("sub #3 val. mismatch: %d", u.val); } int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) { return test_fail("sub #3 copy failed: %d", errno); } if (u.val != 304) { return test_fail("sub #3 val. mismatch: %d", u.val); } return test_note("PASS multi-topic reversed"); }
int uORBTest::UnitTest::test_single() { test_note("try single-topic support"); struct orb_test t, u; int sfd; orb_advert_t ptopic; bool updated; t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } test_note("publish handle %p", ptopic); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } test_note("subscribe fd %d", sfd); u.val = 1; if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(1) failed"); } if (updated) { return test_fail("spurious updated flag"); } t.val = 2; test_note("try publish"); if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { return test_fail("publish failed"); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(2) failed"); } if (!updated) { return test_fail("missing updated flag"); } if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(2) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); } orb_unsubscribe(sfd); int ret = orb_unadvertise(ptopic); if (ret != PX4_OK) { return test_fail("orb_unadvertise failed: %i", ret); } return test_note("PASS single-topic test"); }
int uORBTest::UnitTest::test_multi() { /* this routine tests the multi-topic support */ test_note("try multi-topic support"); struct orb_test t {}, u {}; t.val = 0; int instance0; _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); test_note("advertised"); int instance1; _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); if (instance0 != 0) { return test_fail("mult. id0: %d", instance0); } if (instance1 != 1) { return test_fail("mult. id1: %d", instance1); } t.val = 103; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) { return test_fail("mult. pub0 fail"); } test_note("published"); t.val = 203; if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) { return test_fail("mult. pub1 fail"); } /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { return test_fail("sub #0 copy failed: %d", errno); } if (u.val != 103) { return test_fail("sub #0 val. mismatch: %d", u.val); } int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { return test_fail("sub #1 copy failed: %d", errno); } if (u.val != 203) { return test_fail("sub #1 val. mismatch: %d", u.val); } /* test priorities */ int prio; if (PX4_OK != orb_priority(sfd0, &prio)) { return test_fail("prio #0"); } if (prio != ORB_PRIO_MAX) { return test_fail("prio: %d", prio); } if (PX4_OK != orb_priority(sfd1, &prio)) { return test_fail("prio #1"); } if (prio != ORB_PRIO_MIN) { return test_fail("prio: %d", prio); } if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) { return test_fail("latency test failed"); } orb_unsubscribe(sfd0); orb_unsubscribe(sfd1); return test_note("PASS multi-topic test"); }
static int flow_position_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; printf("[flow position control] starting\n"); uint32_t counter = 0; const float time_scale = powf(10.0f,-6.0f); /* structures */ struct vehicle_status_s vstatus; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; struct filtered_bottom_flow_s filtered_flow; struct vehicle_local_position_s local_pos; struct vehicle_bodyframe_speed_setpoint_s speed_sp; /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); orb_advert_t speed_sp_pub; bool speed_setpoint_adverted = false; /* parameters init*/ struct flow_position_control_params params; struct flow_position_control_param_handles param_handles; parameters_init(¶m_handles); parameters_update(¶m_handles, ¶ms); /* init flow sum setpoint */ float flow_sp_sumx = 0.0f; float flow_sp_sumy = 0.0f; /* init yaw setpoint */ float yaw_sp = 0.0f; /* init height setpoint */ float height_sp = params.height_min; /* height controller states */ bool start_phase = true; bool landing_initialized = false; float landing_thrust_start = 0.0f; /* states */ float integrated_h_error = 0.0f; float last_local_pos_z = 0.0f; bool update_flow_sp_sumx = false; bool update_flow_sp_sumy = false; uint64_t last_time = 0.0f; float dt = 0.0f; // s /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime"); perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); static bool sensors_ready = false; while (!thread_should_exit) { /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { /* polling */ struct pollfd fds[2] = { { .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator { .fd = parameter_update_sub, .events = POLLIN } }; /* wait for a position update, check for exit condition every 500 ms */ int ret = poll(fds, 2, 500); if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); } else if (ret == 0) { /* no return value, ignore */ // printf("[flow position control] no filtered flow updates\n"); } else { /* parameter update available? */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); printf("[flow position control] parameters updated.\n"); } /* only run controller if position/speed changed */ if (fds[0].revents & POLLIN) { perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); if (vstatus.state_machine == SYSTEM_STATE_AUTO) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 /* calc dt */ if(last_time == 0) { last_time = hrt_absolute_time(); continue; } dt = ((float) (hrt_absolute_time() - last_time)) * time_scale; last_time = hrt_absolute_time(); /* update flow sum setpoint */ if (update_flow_sp_sumx) { flow_sp_sumx = filtered_flow.sumx; update_flow_sp_sumx = false; } if (update_flow_sp_sumy) { flow_sp_sumy = filtered_flow.sumy; update_flow_sp_sumy = false; } /* calc new bodyframe speed setpoints */ float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d; float speed_limit_height_factor = height_sp; // the settings are for 1 meter /* overwrite with rc input if there is any */ if(isfinite(manual_pitch) && isfinite(manual_roll)) { if(fabsf(manual_pitch) > params.manual_threshold) { speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor; update_flow_sp_sumx = true; } if(fabsf(manual_roll) > params.manual_threshold) { speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor; update_flow_sp_sumy = true; } } /* limit speed setpoints */ if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) && (speed_body_x >= -params.limit_speed_x * speed_limit_height_factor)) { speed_sp.vx = speed_body_x; } else { if(speed_body_x > params.limit_speed_x * speed_limit_height_factor) speed_sp.vx = params.limit_speed_x * speed_limit_height_factor; if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor) speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor; } if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) && (speed_body_y >= -params.limit_speed_y * speed_limit_height_factor)) { speed_sp.vy = speed_body_y; } else { if(speed_body_y > params.limit_speed_y * speed_limit_height_factor) speed_sp.vy = params.limit_speed_y * speed_limit_height_factor; if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor) speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor; } /* manual yaw change */ if(isfinite(manual_yaw) && isfinite(manual.throttle)) { if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f) { yaw_sp += manual_yaw * params.limit_yaw_step; /* modulo for rotation -pi +pi */ if(yaw_sp < -M_PI_F) yaw_sp = yaw_sp + M_TWOPI_F; else if(yaw_sp > M_PI_F) yaw_sp = yaw_sp - M_TWOPI_F; } } /* forward yaw setpoint */ speed_sp.yaw_sp = yaw_sp; /* manual height control * 0-20%: thrust linear down * 20%-40%: down * 40%-60%: stabilize altitude * 60-100%: up */ float thrust_control = 0.0f; if (isfinite(manual.throttle)) { if (start_phase) { /* control start thrust with stick input */ if (manual.throttle < 0.4f) { /* first 40% for up to feedforward */ thrust_control = manual.throttle / 0.4f * params.thrust_feedforward; } else { /* second 60% for up to feedforward + 10% */ thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward; } /* exit start phase if setpoint is reached */ if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower) { start_phase = false; /* switch to stabilize */ thrust_control = params.thrust_feedforward; } } else { if (manual.throttle < 0.2f) { /* landing initialization */ if (!landing_initialized) { /* consider last thrust control to avoid steps */ landing_thrust_start = speed_sp.thrust_sp; landing_initialized = true; } /* set current height as setpoint to avoid steps */ if (-local_pos.z > params.height_min) height_sp = -local_pos.z; else height_sp = params.height_min; /* lower 20% stick range controls thrust down */ thrust_control = manual.throttle / 0.2f * landing_thrust_start; /* assume ground position here */ if (thrust_control < 0.1f) { /* reset integral if on ground */ integrated_h_error = 0.0f; /* switch to start phase */ start_phase = true; /* reset height setpoint */ height_sp = params.height_min; } } else { /* stabilized mode */ landing_initialized = false; /* calc new thrust with PID */ float height_error = (local_pos.z - (-height_sp)); /* update height setpoint if needed*/ if (manual.throttle < 0.4f) { /* down */ if (height_sp > params.height_min + params.height_rate && fabsf(height_error) < params.limit_height_error) height_sp -= params.height_rate * dt; } if (manual.throttle > 0.6f) { /* up */ if (height_sp < params.height_max && fabsf(height_error) < params.limit_height_error) height_sp += params.height_rate * dt; } /* instead of speed limitation, limit height error (downwards) */ if(height_error > params.limit_height_error) height_error = params.limit_height_error; else if(height_error < -params.limit_height_error) height_error = -params.limit_height_error; integrated_h_error = integrated_h_error + height_error; float integrated_thrust_addition = integrated_h_error * params.height_i; if(integrated_thrust_addition > params.limit_thrust_int) integrated_thrust_addition = params.limit_thrust_int; if(integrated_thrust_addition < -params.limit_thrust_int) integrated_thrust_addition = -params.limit_thrust_int; float height_speed = last_local_pos_z - local_pos.z; float thrust_diff = height_error * params.height_p - height_speed * params.height_d; thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition; /* add attitude component * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM */ // // TODO problem with attitude // if (att.R_valid && att.R[2][2] > 0) // thrust_control = thrust_control / att.R[2][2]; /* set thrust lower limit */ if(thrust_control < params.limit_thrust_lower) thrust_control = params.limit_thrust_lower; } } /* set thrust upper limit */ if(thrust_control > params.limit_thrust_upper) thrust_control = params.limit_thrust_upper; } /* store actual height for speed estimation */ last_local_pos_z = local_pos.z; speed_sp.thrust_sp = thrust_control; speed_sp.timestamp = hrt_absolute_time(); /* publish new speed setpoint */ if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp)) { if(speed_setpoint_adverted) { orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp); } else { speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp); speed_setpoint_adverted = true; } } else { warnx("NaN in flow position controller!"); } } else { /* in manual or stabilized state just reset speed and flow sum setpoint */ speed_sp.vx = 0.0f; speed_sp.vy = 0.0f; flow_sp_sumx = filtered_flow.sumx; flow_sp_sumy = filtered_flow.sumy; if(isfinite(att.yaw)) { yaw_sp = att.yaw; speed_sp.yaw_sp = att.yaw; } if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); } } counter++; }
int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); struct gyro_scale gyro_scale = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, }; int res = OK; /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } if (res == OK) { /* determine gyro mean values */ const unsigned calibration_count = 5000; unsigned calibration_counter = 0; unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = sub_sensor_gyro; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); } else { poll_errcount++; } if (poll_errcount > 1000) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } } close(sub_sensor_gyro); gyro_scale.x_offset /= calibration_count; gyro_scale.y_offset /= calibration_count; gyro_scale.z_offset /= calibration_count; } if (res == OK) { /* check offsets */ if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); res = ERROR; } } if (res == OK) { /* set offset parameters to new values */ if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } #if 0 /* beep on offset calibration end */ mavlink_log_info(mavlink_fd, "gyro offset calibration done"); tune_neutral(); /* scale calibration */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); /* apply new offsets */ fd = open(GYRO_DEVICE_PATH, 0); if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) warn("WARNING: failed to apply new offsets for gyro"); close(fd); unsigned rotations_count = 30; float gyro_integral = 0.0f; float baseline_integral = 0.0f; // XXX change to mag topic orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; uint64_t last_time = hrt_absolute_time(); uint64_t start_time = hrt_absolute_time(); while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; } /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = sub_sensor_combined; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret) { float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; last_time = hrt_absolute_time(); orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); // XXX this is just a proof of concept and needs world / body // transformation and more //math::Vector2f magNav(raw.magnetometer_ga); // calculate error between estimate and measurement // apply declination correction for true heading as well. //float mag = -atan2f(magNav(1),magNav(0)); float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); if (mag > M_PI_F) mag -= 2 * M_PI_F; if (mag < -M_PI_F) mag += 2 * M_PI_F; float diff = mag - mag_last; if (diff > M_PI_F) diff -= 2 * M_PI_F; if (diff < -M_PI_F) diff += 2 * M_PI_F; baseline_integral += diff; mag_last = mag; // Jump through some timing scale hoops to avoid // operating near the 1e6/1e8 max sane resolution of float. gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // } else if (poll_ret == 0) { // /* any poll failure for 1s is a reason to abort */ // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); // return; } } float gyro_scale = baseline_integral / gyro_integral; warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); close(sub_sensor_gyro); mavlink_log_critical(mavlink_fd, "gyro calibration failed"); return ERROR; } /* beep on calibration end */ mavlink_log_info(mavlink_fd, "gyro scale calibration done"); tune_neutral(); #endif if (res == OK) { /* set scale parameters to new values */ if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } } if (res == OK) { /* apply new scaling and offsets */ fd = open(GYRO_DEVICE_PATH, 0); res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; }
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation"); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); /* * Detect if the system is rotating. * * We're detecting this as a general rotation on any axis, not necessary on the one we * asked the user for. This is because we really just need two roughly orthogonal axes * for a good result, so we're not constraining the user more than we have to. */ hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; const float gyro_int_thresh_rad = 0.5f; int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && fabsf(gyro_y_integral) < gyro_int_thresh_rad && fabsf(gyro_z_integral) < gyro_int_thresh_rad) { /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; px4_close(sub_gyro); return result; } /* abort with timeout */ if (hrt_absolute_time() > detection_deadline) { result = calibrate_return_error; warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation."); break; } /* Wait clocking for new data on all gyro */ px4_pollfd_struct_t fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); /* ensure we have a valid first timestamp */ if (last_gyro > 0) { /* integrate */ float delta_t = (gyro.timestamp - last_gyro) / 1e6f; gyro_x_integral += gyro.x * delta_t; gyro_y_integral += gyro.y * delta_t; gyro_z_integral += gyro.z * delta_t; } last_gyro = gyro.timestamp; } } px4_close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; calibration_counter_side = 0; while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; break; } // Wait clocking for new data on all mags px4_pollfd_struct_t fds[max_mags]; size_t fd_count = 0; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data->sub_mag[cur_mag] >= 0) { fds[fd_count].fd = worker_data->sub_mag[cur_mag]; fds[fd_count].events = POLLIN; fd_count++; } } int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { int prev_count[max_mags]; bool rejected = false; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag]; if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); // Check if this measurement is good to go in rejected = rejected || reject_sample(mag.x, mag.y, mag.z, worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], worker_data->calibration_counter_total[cur_mag], calibration_sides * worker_data->calibration_points_perside); worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; worker_data->calibration_counter_total[cur_mag]++; } } // Keep calibration of all mags in lockstep if (rejected) { // Reset counts, since one of the mags rejected the measurement for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; } } else { calibration_counter_side++; // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } } else { poll_errcount++; } if (poll_errcount > worker_data->calibration_points_perside * 3) { result = calibrate_return_error; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); break; } } if (result == calibrate_return_ok) { mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); } return result; }
void Navigator::params_update() { parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); }
int uORBTest::UnitTest::test_queue() { test_note("Testing orb queuing"); struct orb_test_medium t, u; int sfd; orb_advert_t ptopic; bool updated; sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } const int queue_size = 11; t.val = 0; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } orb_check(sfd, &updated); if (!updated) { return test_fail("update flag not set"); } if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } orb_check(sfd, &updated); if (updated) { return test_fail("spurious updated flag"); } #define CHECK_UPDATED(element) \ orb_check(sfd, &updated); \ if (!updated) { \ return test_fail("update flag not set, element %i", element); \ } #define CHECK_NOT_UPDATED(element) \ orb_check(sfd, &updated); \ if (updated) { \ return test_fail("update flag set, element %i", element); \ } #define CHECK_COPY(i_got, i_correct) \ orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ if (i_got != i_correct) { \ return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ } //no messages in the queue anymore test_note(" Testing to write some elements..."); for (int i = 0; i < queue_size - 2; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (int i = 0; i < queue_size - 2; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing overflow..."); int overflow_by = 3; for (int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (int i = 0; i < queue_size; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i + overflow_by); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing underflow..."); for (int i = 0; i < queue_size; ++i) { CHECK_NOT_UPDATED(i); CHECK_COPY(u.val, queue_size + overflow_by - 1); } t.val = 943; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); #undef CHECK_COPY #undef CHECK_UPDATED #undef CHECK_NOT_UPDATED orb_unadvertise(ptopic); return test_note("PASS orb queuing"); }
void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); navigation_capabilities_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { PX4_WARN("navigator timed out"); } continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_WARN("nav: poll error %d, %d", pret, errno); continue; } global_pos_available_once = true; perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_capabilities_sub, &updated); if (updated) { navigation_capabilities_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = NAN; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = cmd.param5 / (double)1e7; rep->current.lon = cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: if (_nav_caps.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; _pos_sp_triplet_published_invalid_once = false; } else { _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; return; }
int uORBTest::UnitTest::pubsublatency_main() { /* poll on test topic and output latency */ float latency_integral = 0.0f; /* wakeup source(s) */ px4_pollfd_struct_t fds[3]; int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); struct orb_test_large t; /* clear all ready flags */ orb_copy(ORB_ID(orb_test), test_multi_sub, &t); orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); fds[0].fd = test_multi_sub; fds[0].events = POLLIN; fds[1].fd = test_multi_sub_medium; fds[1].events = POLLIN; fds[2].fd = test_multi_sub_large; fds[2].events = POLLIN; const unsigned maxruns = 1000; unsigned timingsgroup = 0; int current_value = t.val; int num_missed = 0; // timings has to be on the heap to keep frame size below 2048 bytes unsigned *timings = new unsigned[maxruns]; unsigned timing_min = 9999999, timing_max = 0; for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test), test_multi_sub, &t); timingsgroup = 0; } else if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); timingsgroup = 1; } else if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); timingsgroup = 2; } if (pret < 0) { PX4_ERR("poll error %d, %d", pret, errno); continue; } num_missed += t.val - current_value - 1; current_value = t.val; unsigned elt = (unsigned)hrt_elapsed_time(&t.time); latency_integral += elt; timings[i] = elt; if (elt > timing_max) { timing_max = elt; } if (elt < timing_min) { timing_min = elt; } } orb_unsubscribe(test_multi_sub); orb_unsubscribe(test_multi_sub_medium); orb_unsubscribe(test_multi_sub_large); if (pubsubtest_print) { char fname[32]; sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup); FILE *f = fopen(fname, "w"); if (f == nullptr) { PX4_ERR("Error opening file!"); delete[] timings; return PX4_ERROR; } for (unsigned i = 0; i < maxruns; i++) { fprintf(f, "%u\n", timings[i]); } fclose(f); } float std_dev = 0.f; float mean = latency_integral / maxruns; for (unsigned i = 0; i < maxruns; i++) { float diff = (float)timings[i] - mean; std_dev += diff * diff; } delete[] timings; PX4_INFO("mean: %8.4f us", static_cast<double>(mean)); PX4_INFO("std dev: %8.4f us", static_cast<double>(sqrtf(std_dev / (maxruns - 1)))); PX4_INFO("min: %3i us", timing_min); PX4_INFO("max: %3i us", timing_max); PX4_INFO("missed topic updates: %i", num_missed); pubsubtest_passed = true; if (static_cast<float>(latency_integral / maxruns) > 100.0f) { pubsubtest_res = PX4_ERROR; } else { pubsubtest_res = PX4_OK; } return pubsubtest_res; }
void FlightTasks::_updateCommand() { // lazy subscription to command topic if (_sub_vehicle_command < 0) { _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); } // check if there's any new command bool updated = false; orb_check(_sub_vehicle_command, &updated); if (!updated) { return; } // get command struct vehicle_command_s command; orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); // check what command it is FlightTaskIndex desired_task = switchVehicleCommand(command.command); if (desired_task == FlightTaskIndex::None) { // ignore all unkown commands return; } // switch to the commanded task int switch_result = switchTask(desired_task); uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; // if we are in/switched to the desired task if (switch_result >= 0) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; // if the task is running apply parameters to it and see if it rejects if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; // if we just switched and parameters are not accepted, go to failsafe if (switch_result == 1) { switchTask(FlightTaskIndex::ManualPosition); cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; } } } // send back acknowledgment vehicle_command_ack_s command_ack = {}; command_ack.command = command.command; command_ack.result = cmd_result; command_ack.result_param1 = switch_result; command_ack.target_system = command.source_system; command_ack.target_component = command.source_component; if (_pub_vehicle_command_ack == nullptr) { _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); } else { orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); } }
int uORBTest::UnitTest::test_queue_poll_notify() { test_note("Testing orb queuing (poll & notify)"); struct orb_test_medium t; int sfd; if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { return test_fail("subscribe failed: %d", errno); } _thread_should_exit = false; char *const args[1] = { nullptr }; int pubsub_task = px4_task_spawn_cmd("uorb_test_queue", SCHED_DEFAULT, SCHED_PRIORITY_MIN + 5, 1500, (px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry, args); if (pubsub_task < 0) { return test_fail("failed launching task"); } int next_expected_val = 0; px4_pollfd_struct_t fds[1]; fds[0].fd = sfd; fds[0].events = POLLIN; while (!_thread_should_exit) { int poll_ret = px4_poll(fds, 1, 500); if (poll_ret == 0) { if (_thread_should_exit) { break; } return test_fail("poll timeout"); } else if (poll_ret < 0) { return test_fail("poll error (%d, %d)", poll_ret, errno); } if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t); if (next_expected_val != t.val) { return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val); } ++next_expected_val; } } if (_num_messages_sent != next_expected_val) { return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)", _num_messages_sent, next_expected_val); } return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val); }
pthread_addr_t UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); Lock lock(_subnode_mutex); /* Copy any firmware bundled in the ROMFS to the appropriate location on the SD card, unless the user has copied other firmware for that device. */ unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); /* the subscribe call needs to happen in the same thread, * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; memset(_param_counts, 0, sizeof(_param_counts)); _esc_enumeration_active = false; memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; _node_info_retriever.invalidateAll(); } const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); if (spin_res < 0) { warnx("node spin error %i", spin_res); } // Check for parameter requests (get/set/list) bool param_request_ready; orb_check(param_request_sub, ¶m_request_ready); if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { struct uavcan_parameter_request_s request; orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); if (_param_counts[request.node_id]) { /* * We know how many parameters are exposed by this node, so * process the request. */ if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { req.index = request.param_index; } else { req.name = (char*)request.param_id; } int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; _param_index = request.param_index; } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { req.index = request.param_index; } else { req.name = (char*)request.param_id; } if (request.param_type == MAV_PARAM_TYPE_REAL32) { req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value; } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value; } else { req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value; } // Set the dirty bit for this node set_node_params_dirty(request.node_id); int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; _param_index = request.param_index; } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { // This triggers the _param_list_in_progress case below. _param_index = 0; _param_list_in_progress = true; _param_list_node_id = request.node_id; _param_list_all_nodes = false; warnx("UAVCAN command bridge: starting component-specific param list"); } } else if (request.node_id == MAV_COMP_ID_ALL) { if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { /* * This triggers the _param_list_in_progress case below, * but additionally iterates over all active nodes. */ _param_index = 0; _param_list_in_progress = true; _param_list_node_id = get_next_active_node_id(1); _param_list_all_nodes = true; warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } } } else { /* * Need to know how many parameters this node has before we can * continue; count them now and then process the request. */ param_count(request.node_id); } } // Handle parameter listing index/node ID advancement if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { if (_param_index >= _param_counts[_param_list_node_id]) { warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); // Reached the end of the current node's parameter set. _param_list_in_progress = false; if (_param_list_all_nodes) { // We're listing all parameters for all nodes -- get the next node ID uint8_t next_id = get_next_active_node_id(_param_list_node_id); if (next_id < 128) { _param_list_node_id = next_id; /* * If there is a next node ID, check if that node's parameters * have been counted before. If not, do it now. */ if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } // Keep on listing. _param_index = 0; _param_list_in_progress = true; warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); } } } } // Check if we're still listing, and need to get the next parameter if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { // Ready to request the next value -- _param_index is incremented // after each successful fetch by cb_getset uavcan::protocol::param::GetSet::Request req; req.index = _param_index; int call_res = _param_getset_client.call(_param_list_node_id, req); if (call_res < 0) { _param_list_in_progress = false; warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; } } // Check for ESC enumeration commands bool cmd_ready; orb_check(cmd_sub, &cmd_ready); if (cmd_ready && !_cmd_in_progress) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { int command_id = static_cast<int>(cmd.param1 + 0.5f); int node_id = static_cast<int>(cmd.param2 + 0.5f); int call_res; warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); switch (command_id) { case 0: case 1: { _esc_enumeration_active = command_id; _esc_enumeration_index = 0; _esc_count = 0; uavcan::protocol::enumeration::Begin::Request req; req.parameter_name = "esc_index"; req.timeout_sec = _esc_enumeration_active ? 65535 : 0; call_res = _enumeration_client.call(get_next_active_node_id(1), req); if (call_res < 0) { warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); } break; } default: { warnx("UAVCAN command bridge: unknown command ID %d", command_id); break; } } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { int command_id = static_cast<int>(cmd.param1 + 0.5f); warnx("UAVCAN command bridge: received storage command ID %d", command_id); switch (command_id) { case 1: { // Param save request int node_id; node_id = get_next_dirty_node_id(1); if (node_id < 128) { _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; param_opcode(node_id); } break; } case 2: { // Command is a param erase request -- apply it to all active nodes by setting the dirty bit _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; for (int i = 1; i < 128; i = get_next_active_node_id(i)) { set_node_params_dirty(i); } param_opcode(get_next_dirty_node_id(1)); break; } } } } // Shut down once armed // TODO (elsewhere): start up again once disarmed? bool updated; orb_check(armed_sub, &updated); if (updated) { struct actuator_armed_s armed; orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); if (armed.armed && !armed.lockdown) { warnx("UAVCAN command bridge: system armed, exiting now."); break; } } } _subnode_thread_should_exit = false; warnx("exiting"); return (pthread_addr_t) 0; }
void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); }
void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude and control state topics */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { // the tailsitter recovery instance has not been created, thus, the vehicle // is not a tailsitter, do normal attitude control control_attitude(dt); } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */ for (int i = 0; i < 3; i++) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } _control_task = -1; return; }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ float debugOutput[4] = { 0.0f }; int overloadcounter = 19; /* Initialize filter */ AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); gps.eph = 100000; gps.epv = 100000; struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vision estimate */ int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); struct vision_position_estimate vision {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { warnx("WARNING: Not getting sensors - sensor app running?"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; bool vel_valid = false; if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; vel(0) = gps.vel_n_m_s; vel(1) = gps.vel_e_m_s; vel(2) = gps.vel_d_m_s; } } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } bool vision_updated = false; orb_check(vision_sub, &vision_updated); if (vision_updated) { orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rvis * v; z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); } else { z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; } uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; last_run = now; if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here // if (overloadcounter == 20) { // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); // overloadcounter = 0; // } overloadcounter++; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } /* Call the estimator */ AttitudeEKF(false, // approx_prediction (unsigned char)ekf_params.use_moment_inertia, update_vect, dt, z_k, ekf_params.q[0], // q_rotSpeed, ekf_params.q[1], // q_rotAcc ekf_params.q[2], // q_acc ekf_params.q[3], // q_mag ekf_params.r[0], // r_gyro ekf_params.r[1], // r_accel ekf_params.r[2], // r_mag ekf_params.moment_inertia_J, x_aposteriori, P_aposteriori, Rot_matrix, euler, debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; math::Quaternion q; q.from_dcm(R); /* copy rotation matrix */ memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
int sdlog2_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { warnx("failed to open MAVLink log stream, start mavlink app first."); } /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); if (r == 0) { sleep_delay = 0; } else { sleep_delay = 1000000 / r; } } break; case 'b': { unsigned long s = strtoul(optarg, NULL, 10); if (s < 1) { s = 1; } log_buffer_size = 1024 * s; } break; case 'e': log_on_start = true; break; case 'a': log_when_armed = true; break; case '?': if (optopt == 'c') { warnx("Option -%c requires an argument.", optopt); } else if (isprint(optopt)) { warnx("Unknown option `-%c'.", optopt); } else { warnx("Unknown option character `\\x%x'.", optopt); } default: sdlog2_usage("unrecognized flag"); errx(1, "exiting."); } } if (!file_exist(mountpoint)) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } if (create_logfolder()) { errx(1, "unable to create logging folder, exiting."); } const char *converter_in = "/etc/logging/conv.zip"; char *converter_out = malloc(120); sprintf(converter_out, "%s/conv.zip", folder_path); if (file_copy(converter_in, converter_out)) { errx(1, "unable to copy conversion scripts, exiting."); } free(converter_out); /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); /* initialize log buffer with specified size */ warnx("log buffer size: %i bytes.", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { errx(1, "can't allocate log buffer, exiting."); } struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; struct actuator_controls_effective_s act_controls_effective; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); struct { int cmd_sub; int status_sub; int sensor_sub; int att_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; int act_controls_sub; int act_controls_effective_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; int global_pos_sp_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; int esc_sub; int global_vel_sp_sub; } subs; /* log message buffer: header + body */ #pragma pack(push, 1) struct { LOG_PACKET_HEADER; union { struct log_TIME_s log_TIME; struct log_ATT_s log_ATT; struct log_ATSP_s log_ATSP; struct log_IMU_s log_IMU; struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) }; #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ const ssize_t fdsc = 20; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE --- */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE SETPOINT --- */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); fds[fdsc_count].fd = subs.att_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- RATES SETPOINT --- */ subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); fds[fdsc_count].fd = subs.rates_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR OUTPUTS --- */ subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); fds[fdsc_count].fd = subs.act_outputs_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL --- */ subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); fds[fdsc_count].fd = subs.act_controls_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL EFFECTIVE --- */ subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); fds[fdsc_count].fd = subs.act_controls_effective_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL POSITION SETPOINT --- */ subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); fds[fdsc_count].fd = subs.local_pos_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL POSITION --- */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); fds[fdsc_count].fd = subs.global_pos_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- RC CHANNELS --- */ subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); fds[fdsc_count].fd = subs.rc_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- AIRSPEED --- */ subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); fds[fdsc_count].fd = subs.esc_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL VELOCITY SETPOINT --- */ subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); fds[fdsc_count].fd = subs.global_vel_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); fdsc_count = fdsc; } /* * set up poll to block for new data, * wait for a maximum of 1000 ms */ const int poll_timeout = 1000; thread_running = true; /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ uint16_t gyro_counter = 0; uint16_t accelerometer_counter = 0; uint16_t magnetometer_counter = 0; uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; /* enable logging on start if needed */ if (log_on_start) sdlog2_start_log(); while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { warnx("ERROR: poll error, stop logging."); main_thread_should_exit = true; } else if (poll_ret > 0) { /* check all data subscriptions only if logging enabled, * logging_enabled can be changed while checking vehicle_command and vehicle_status */ bool check_data = logging_enabled; int ifds = 0; int handled_topics = 0; /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); handle_command(&buf.cmd); handled_topics++; } /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { handle_status(&buf_status); } handled_topics++; } if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ log_msg.msg_type = LOG_TIME_MSG; log_msg.body.log_TIME.t = hrt_absolute_time(); LOGBUFFER_WRITE_AND_COUNT(TIME); /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; log_msg.body.log_GPS.eph = buf.gps_pos.eph_m; log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; log_msg.body.log_GPS.lat = buf.gps_pos.lat; log_msg.body.log_GPS.lon = buf.gps_pos.lon; log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f; log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; LOGBUFFER_WRITE_AND_COUNT(GPS); } /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); bool write_IMU = false; bool write_SENS = false; if (buf.sensor.gyro_counter != gyro_counter) { gyro_counter = buf.sensor.gyro_counter; write_IMU = true; } if (buf.sensor.accelerometer_counter != accelerometer_counter) { accelerometer_counter = buf.sensor.accelerometer_counter; write_IMU = true; } if (buf.sensor.magnetometer_counter != magnetometer_counter) { magnetometer_counter = buf.sensor.magnetometer_counter; write_IMU = true; } if (buf.sensor.baro_counter != baro_counter) { baro_counter = buf.sensor.baro_counter; write_SENS = true; } if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { differential_pressure_counter = buf.sensor.differential_pressure_counter; write_SENS = true; } if (write_IMU) { log_msg.msg_type = LOG_IMU_MSG; log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; LOGBUFFER_WRITE_AND_COUNT(IMU); } if (write_SENS) { log_msg.msg_type = LOG_SENS_MSG; log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } } /* --- ATTITUDE --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); log_msg.msg_type = LOG_ATT_MSG; log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); log_msg.msg_type = LOG_ATSP_MSG; log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; LOGBUFFER_WRITE_AND_COUNT(ATSP); } /* --- RATES SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp); log_msg.msg_type = LOG_ARSP_MSG; log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; LOGBUFFER_WRITE_AND_COUNT(ARSP); } /* --- ACTUATOR OUTPUTS --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); } /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); log_msg.msg_type = LOG_ATTC_MSG; log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- ACTUATOR CONTROL EFFECTIVE --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); // TODO not implemented yet } /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); log_msg.msg_type = LOG_LPOS_MSG; log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; log_msg.body.log_LPOS.z = buf.local_pos.z; log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } /* --- LOCAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp); log_msg.msg_type = LOG_LPSP_MSG; log_msg.body.log_LPSP.x = buf.local_pos_sp.x; log_msg.body.log_LPSP.y = buf.local_pos_sp.y; log_msg.body.log_LPSP.z = buf.local_pos_sp.z; log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; LOGBUFFER_WRITE_AND_COUNT(LPSP); } /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; log_msg.body.log_GPOS.lat = buf.global_pos.lat; log_msg.body.log_GPOS.lon = buf.global_pos.lon; log_msg.body.log_GPOS.alt = buf.global_pos.alt; log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); log_msg.msg_type = LOG_GPSP_MSG; log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; LOGBUFFER_WRITE_AND_COUNT(GPSP); } /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); // TODO not implemented yet } /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); log_msg.msg_type = LOG_FLOW_MSG; log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); } /* --- RC CHANNELS --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc); log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); LOGBUFFER_WRITE_AND_COUNT(RC); } /* --- AIRSPEED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; LOGBUFFER_WRITE_AND_COUNT(AIRS); } /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; log_msg.body.log_ESC.esc_count = buf.esc.esc_count; log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; log_msg.body.log_ESC.esc_num = i; log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; LOGBUFFER_WRITE_AND_COUNT(ESC); } } /* --- GLOBAL VELOCITY SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); log_msg.msg_type = LOG_GVSP_MSG; log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; LOGBUFFER_WRITE_AND_COUNT(GVSP); } /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } /* unlock, now the writer thread may run */ pthread_mutex_unlock(&logbuffer_mutex); } if (use_sleep) { usleep(sleep_delay); } } if (logging_enabled) sdlog2_stop_log(); pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); free(lb.data); warnx("exiting."); thread_running = false; return 0; }
void Navigator::navigation_capabilities_update() { orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); }
void FixedwingPositionControl::task_main() { /* inform about start */ warnx("Initializing.."); fflush(stdout); /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); /* abort on a nonzero return value from the parameter init */ if (parameters_update()) { /* parameter setup went wrong, abort */ warnx("aborting startup due to errors."); _task_should_exit = true; } /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { /* XXX Hack to get mavlink output going */ if (_mavlink_fd < 0) { /* try to open the mavlink log device every once in a while */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); // XXX add timestamp check _global_pos_valid = true; vehicle_attitude_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); range_finder_poll(); // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); } else { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } /* XXX check if radius makes sense here */ float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; navigation_capabilities_publish(); } } } perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _exit(0); }
void Navigator::vehicle_land_detected_update() { orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected); }
void Navigator::sensor_combined_update() { orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); }
int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ struct airspeed_scale airscale = { 0.0f, 1.0f, }; bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { warn("FAILED to set scale / offsets for airspeed"); mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } } diff_pres_offset = diff_pres_offset / calibration_count; if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } }