From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 22:44:05 +0100 Subject: [PATCH 001/105] ACRO mode implemented --- src/modules/commander/commander.cpp | 35 +++++- .../commander/state_machine_helper.cpp | 4 + .../mc_att_control/mc_att_control_main.cpp | 102 +++++++++++++----- .../mc_att_control/mc_att_control_params.c | 3 + src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++ .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/rc_channels.h | 11 +- src/modules/uORB/topics/vehicle_status.h | 8 ++ 9 files changed, 144 insertions(+), 35 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..89294fc76c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; - main_states_str[3] = "AUTO"; + main_states_str[1] = "ACRO"; + main_states_str[2] = "SEATBELT"; + main_states_str[3] = "EASY"; + main_states_str[4] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* acro switch */ + if (!isfinite(sp_man->acro_switch)) { + status->acro_switch = ACRO_SWITCH_NONE; + + } else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) { + status->acro_switch = ACRO_SWITCH_ACRO; + + } else { + status->acro_switch = ACRO_SWITCH_NORMAL; + } } transition_result_t @@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status) switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (status->acro_switch == ACRO_SWITCH_ACRO) { + res = main_state_transition(status, MAIN_STATE_ACRO); + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1600,6 +1616,17 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..fd966a0682 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -230,6 +230,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0accb8552..5f862652a3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -161,7 +161,12 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t roll_scale_acro; + param_t pitch_scale_acro; + param_t yaw_scale_acro; + param_t rc_scale_roll; + param_t rc_scale_pitch; param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ @@ -171,8 +176,9 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + math::Vector<3> scale_acro; /**< scale for ACRO mode */ - float rc_scale_yaw; + math::Vector<3> rc_scale; /**< scale for MANUAL mode */ } _params; /** @@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.scale_acro.zero(); + _params.rc_scale.zero(); _R_sp.identity(); _R.identity(); @@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO"); + _params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO"); + _params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_scale_acro, &v); + _params.scale_acro(0) = v; + param_get(_params_handles.rc_scale_roll, &v); + _params.rc_scale(0) = v; /* pitch */ param_get(_params_handles.pitch_p, &v); @@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_scale_acro, &v); + _params.scale_acro(1) = v; + param_get(_params_handles.rc_scale_pitch, &v); + _params.rc_scale(1) = v; /* yaw */ param_get(_params_handles.yaw_p, &v); @@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + param_get(_params_handles.yaw_scale_acro, &v); + _params.scale_acro(2) = v; + param_get(_params_handles.rc_scale_yaw, &v); + _params.rc_scale(2) = v; return OK; } @@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ + vehicle_manual_poll(); if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ @@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2); + if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2); if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; } else { yaw_sp_move_rate += YAW_DEADZONE; } - yaw_sp_move_rate *= _params.rc_scale_yaw; + yaw_sp_move_rate *= _params.rc_scale(2); _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main() parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control, ACRO mode */ + vehicle_manual_poll(); + + _rates_sp(0) = _manual_control_sp.roll; + _rates_sp(1) = _manual_control_sp.pitch; + _rates_sp(2) = _manual_control_sp.yaw; + + /* rescale controls for ACRO mode */ + _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); + _thrust_sp = _manual_control_sp.throttle; + + /* 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 { + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bbc..f3a4022c84 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a0..e72b48a881 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bd665b592b..50ddec8a94 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,6 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_acro_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,6 +297,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_acro_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -515,6 +517,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -669,6 +672,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("Failed getting acro sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -700,6 +707,7 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1290,6 +1298,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.acro_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1428,6 +1437,11 @@ Sensors::rc_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* acro switch input */ + if (_rc.function[ACRO] >= 0) { + manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled); + } + /* return switch input */ if (_rc.function[RETURN] >= 0) { manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..190dc01c8e 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,6 +60,7 @@ struct manual_control_setpoint_s { float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + float acro_switch; /**< acro 2 position switch (optional): normal, acro */ /** * Any of the channels below may not be available and be set to NaN diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef150..beb7008ab9 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION ASSISTED = 6, MISSION = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1b3639e309..5cb0bd8a20 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,6 +63,7 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, + MAIN_STATE_ACRO, MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, @@ -116,6 +117,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + ACRO_SWITCH_NONE = 0, + ACRO_SWITCH_NORMAL, + ACRO_SWITCH_ACRO +} acro_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -192,6 +199,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + acro_switch_pos_t acro_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:25:29 +0100 Subject: [PATCH 002/105] commander: allow disarming in ACRO without landing (as in MANUAL) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89294fc76c..30f75b9fb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { From 21300874ac6d337d85c49704e9233fdd17192171 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 23:07:48 +0100 Subject: [PATCH 003/105] mc_att_control: reset yaw setpoint after ACRO --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 21e1016625..77531cbb92 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -802,6 +802,8 @@ MulticopterAttitudeControl::task_main() _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); _thrust_sp = _manual_control_sp.throttle; + _reset_yaw_sp = true; + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); From 6e7136c6b3f953c26cf75c9f8b777a6a7c84ea9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:50:26 +0100 Subject: [PATCH 004/105] rc_channels topic: bug fixed; sensors: minor cleanup --- src/modules/sensors/sensors.cpp | 2 +- src/modules/uORB/topics/rc_channels.h | 34 +++++++++++++-------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b37a744ca2..73f14225de 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -674,7 +674,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { - warnx("Failed getting acro sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index beb7008ab9..36106751e2 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -45,12 +45,12 @@ /** * The number of RC channel inputs supported. * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 15. + * leaving at a sane value of 16. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAPPED_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 16 /** * This defines the mapping of the RC functions. @@ -60,21 +60,21 @@ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - ACRO = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 14, + ROLL, + PITCH, + YAW, + MODE, + RETURN, + ASSISTED, + MISSION, + OFFBOARD_MODE, + ACRO, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:55:12 +0100 Subject: [PATCH 005/105] mavlink: custom mode ACRO added --- src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9c..d69ab30677 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f7..22465a3dae 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: [PATCH 006/105] mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 7 +-- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217de..41670e2474 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a2966..819dcf9c3a 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,14 +59,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +76,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c572972b58..11181ff642 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: [PATCH 007/105] mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e2474..b41e9d355d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: [PATCH 008/105] mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355d..02e35f1a8b 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) From a4724acf82e48d914bfe6397340454505e14decf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 17:26:42 +0200 Subject: [PATCH 009/105] autostart 10016_3dr_iris: yaw PID parameters updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index f11aa704eb..084dff140f 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -18,9 +18,9 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAW_P 2.5 + param set MC_YAWRATE_P 0.25 + param set MC_YAWRATE_I 0.25 param set MC_YAWRATE_D 0.0 param set BAT_V_SCALING 0.00989 From af1af1e22ddd4bcd55fe9eaaf98f4a640329a4c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 18:38:30 +0200 Subject: [PATCH 010/105] Port expander driver first hacky version --- makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/drv_io_expander.h | 65 +++ src/drivers/pca8574/module.mk | 6 + src/drivers/pca8574/pca8574.cpp | 569 ++++++++++++++++++++++++++ 5 files changed, 642 insertions(+) create mode 100644 src/drivers/drv_io_expander.h create mode 100644 src/drivers/pca8574/module.mk create mode 100644 src/drivers/pca8574/pca8574.cpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..602b0af3fb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl +MODULES += drivers/pca8574 # Needs to be burned to the ground and re-written; for now, diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d7..0dcfd04f22 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -24,6 +24,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 +MODULES += drivers/pca8574 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h new file mode 100644 index 0000000000..2705d6f9e1 --- /dev/null +++ b/src/drivers/drv_io_expander.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_io_expander.h + * + * IO expander device API + */ + +#pragma once + +#include +#include + +/* + * ioctl() definitions + */ + +#define _IOXIOCBASE (0x2800) +#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) + +/** enable the device */ +#define IOX_ENABLE _IOXIOC(1) + +/** set constant values */ +#define IOX_SET_VALUE _IOXIOC(2) + +/** set constant values */ +#define IOX_SET_MODE _IOXIOC(3) + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +enum IOX_MODE { + IOX_MODE_OFF, + IOX_MODE_ON +}; diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk new file mode 100644 index 0000000000..825ee9bb7a --- /dev/null +++ b/src/drivers/pca8574/module.mk @@ -0,0 +1,6 @@ +# +# PCA8574 driver for RGB LED +# + +MODULE_COMMAND = pca8574 +SRCS = pca8574.cpp diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp new file mode 100644 index 0000000000..ce882b6367 --- /dev/null +++ b/src/drivers/pca8574/pca8574.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pca8574.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#define PCA8574_ONTIME 120 +#define PCA8574_OFFTIME 120 +#define PCA8574_DEVICE_PATH "/dev/pca8574" + +#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class PCA8574 : public device::I2C +{ +public: + PCA8574(int bus, int pca8574); + virtual ~PCA8574(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + work_s _work; + + uint8_t _values[8]; + float _brightness; + + enum IOX_MODE _mode; + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_values(); +}; + +/* for now, we only support one PCA8574 */ +namespace +{ +PCA8574 *g_pca8574; +} + +void pca8574_usage(); + +extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); + +PCA8574::PCA8574(int bus, int pca8574) : + I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _mode(IOX_MODE_OFF), + _values({}), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +PCA8574::~PCA8574() +{ +} + +int +PCA8574::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + + /* kick it in */ + _should_run = true; + _led_interval = 80; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); + + return OK; +} + +int +PCA8574::probe() +{ + + return send_led_enable(false); +} + +int +PCA8574::info() +{ + int ret = OK; + + return ret; +} + +int +PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): + { + /* set the specified color */ + unsigned prev = _values[cmd - IOX_SET_VALUE]; + _values[cmd - IOX_SET_VALUE] = arg; + if (_values[cmd - IOX_SET_VALUE] != prev) { + // XXX will be done with a change flag + send_led_values(); + } + return OK; + } + + case IOX_ENABLE: + send_led_enable(arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +PCA8574::led_trampoline(void *arg) +{ + PCA8574 *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +PCA8574::led() +{ + if (!_should_run) { + _running = false; + return; + } + + // switch (_mode) { + // case PCA8574_MODE_BLINK_SLOW: + // case PCA8574_MODE_BLINK_NORMAL: + // case PCA8574_MODE_BLINK_FAST: + // if (_counter >= 2) + // _counter = 0; + + // send_led_enable(_counter == 0); + + // break; + + // case PCA8574_MODE_BREATHE: + + // if (_counter >= 62) + // _counter = 0; + + // int n; + + // if (_counter < 32) { + // n = _counter; + + // } else { + // n = 62 - _counter; + // } + + // _brightness = n * n / (31.0f * 31.0f); + // send_led_rgb(); + // break; + + // case PCA8574_MODE_PATTERN: + + // /* don't run out of the pattern array and stop if the next frame is 0 */ + // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + // _counter = 0; + + // set_color(_pattern.color[_counter]); + // send_led_rgb(); + // _led_interval = _pattern.duration[_counter]; + // break; + + // default: + // break; + // } + + + // we count only seven states + _counter &= 0xF; + _counter++; + + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values[i] = 1; + } else { + _values[i] = 0; + } + } + + send_led_values(); + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); +} + +// /** +// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) +// */ +// void +// PCA8574::set_mode(pca8574_mode_t mode) +// { +// if (mode != _mode) { +// _mode = mode; + +// switch (mode) { +// // case PCA8574_MODE_OFF: +// // _should_run = false; +// // send_led_enable(false); +// // break; + +// // case PCA8574_MODE_ON: +// // _brightness = 1.0f; +// // send_led_rgb(); +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_BLINK_SLOW: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 2000; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_NORMAL: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 500; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_FAST: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 100; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BREATHE: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 25; +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_PATTERN: +// // _should_run = true; +// // _counter = 0; +// // _brightness = 1.0f; +// // send_led_enable(true); +// // break; + +// default: +// warnx("mode unknown"); +// break; +// } + +// /* if it should run now, start the workq */ +// if (_should_run && !_running) { +// _running = true; +// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); +// } + +// } +// } + +/** + * Sent ENABLE flag to LED driver + */ +int +PCA8574::send_led_enable(bool enable) +{ + uint8_t msg; + + if (enable) { + /* active low */ + msg = 0x00; + } else { + /* active low, so off */ + msg = 0xFF; + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); + + return ret; +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +PCA8574::send_led_values() +{ + uint8_t msg = 0; + + for (int i = 0; i < 8; i++) { + if (_values[i]) { + msg |= (1 << i); + } + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); +} + +// int +// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +// { +// uint8_t result[2]; +// int ret; + +// ret = transfer(nullptr, 0, &result[0], 2); + +// if (ret == OK) { +// on = result[0] & SETTING_ENABLE; +// powersave = !(result[0] & SETTING_NOT_POWERSAVE); +// /* XXX check, looks wrong */ +// r = (result[0] & 0x0f) << 4; +// g = (result[1] & 0xf0); +// b = (result[1] & 0x0f) << 4; +// } + +// return ret; +// } + +void +pca8574_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca8574_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int pca8574adr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + pca8574adr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca8574_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca8574_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca8574 != nullptr) + errx(1, "already started"); + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); + + if (g_pca8574 != nullptr && OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + } + + if (g_pca8574 == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_pca8574 == nullptr) { + g_pca8574 = new PCA8574(i2cdevice, pca8574adr); + + if (g_pca8574 == nullptr) + errx(1, "new failed"); + + if (OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_pca8574 == nullptr) { + warnx("not started"); + pca8574_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_VALUE, 255); + // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_pca8574->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } + + if (!strcmp(verb, "val")) { + if (argc < 4) { + errx(1, "Usage: pca8574 val "); + } + + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned val = strtol(argv[3], NULL, 0); + ret = ioctl(fd, (IOX_SET_VALUE+channel), val); + ret = ioctl(fd, IOX_ENABLE, 1); + close(fd); + exit(ret); + } + + pca8574_usage(); + exit(0); +} From b60964eb9c1e24b14c0cbf4527bf6b7e4bb5fd40 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 13:27:53 +0200 Subject: [PATCH 011/105] Multirotor mixer: more careful limiting --- .../systemlib/mixer/mixer_multirotor.cpp | 70 +++++++++---------- 1 file changed, 33 insertions(+), 37 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index bf77795d5c..740a22781e 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -282,56 +282,52 @@ MultirotorMixer::mix(float *outputs, unsigned space) float yaw = get_control(0, 2) * _yaw_scale; float thrust = get_control(0, 3); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); - float max = 0.0f; + float min_out = 0.0f; + float max_out = 0.0f; float fixup_scale; - /* use an output factor to prevent too strong control signals at low throttle */ - float min_thrust = 0.05f; - float max_thrust = 1.0f; - float startpoint_full_control = 0.40f; - float output_factor; - - /* keep roll, pitch and yaw control to 0 below min thrust */ - if (thrust <= min_thrust) { - output_factor = 0.0f; - /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ - - } else if (thrust < startpoint_full_control && thrust > min_thrust) { - output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); - /* and then stay at full control */ - - } else { - output_factor = max_thrust; - } - - roll *= output_factor; - pitch *= output_factor; - yaw *= output_factor; - - - /* perform initial mix pass yielding un-bounded outputs */ + /* perform initial mix pass yielding unbounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { - float tmp = roll * _rotors[i].roll_scale + + float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale + + yaw * _rotors[i].yaw_scale + thrust; - if (tmp > max) - max = tmp; + if (out < min_out) { + min_out = out; + } + if (out > max_out) { + max_out = out; + } - outputs[i] = tmp; + outputs[i] = out; } - /* scale values into the -1.0 - 1.0 range */ - if (max > 1.0f) { - fixup_scale = 2.0f / max; + /* scale down controls if some outputs are negative, keep total thrust */ + if (min_out < 0.0) { + float scale_in = thrust / (thrust - min_out); + + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = scale_in * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale) + + thrust; + } + } + + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; } else { - fixup_scale = 2.0f; + scale_out = 1.0f; } - for (unsigned i = 0; i < _rotor_count; i++) - outputs[i] = -1.0f + (outputs[i] * fixup_scale); + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = -1.0f + (outputs[i] * 2 * scale_out); + } /* ensure outputs are out of the deadband */ for (unsigned i = 0; i < _rotor_count; i++) From ae1faa6de6d6952af73a8a9367625fbf96822fe1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 13:45:43 +0200 Subject: [PATCH 012/105] MC mixer input limiting implemented. --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 740a22781e..2af9d913d3 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -67,6 +67,11 @@ namespace { +float constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + /* * These tables automatically generated by multi_tables - do not edit. */ @@ -276,11 +281,11 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl unsigned MultirotorMixer::mix(float *outputs, unsigned space) { - float roll = get_control(0, 0) * _roll_scale; + float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); - float pitch = get_control(0, 1) * _pitch_scale; - float yaw = get_control(0, 2) * _yaw_scale; - float thrust = get_control(0, 3); + float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); + float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); + float thrust = constrain(get_control(0, 3), 0.0f, 1.0f); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; From b9b84b08b79dd6661905cfd5d4fa8578ea392bec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 14:01:55 +0200 Subject: [PATCH 013/105] Multirotor mixer: limit yaw first, then roll/pitch --- .../systemlib/mixer/mixer_multirotor.cpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2af9d913d3..672784f468 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -289,15 +289,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float fixup_scale; + float scale_yaw = 1.0f; - /* perform initial mix pass yielding unbounded outputs */ + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale + thrust; + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ if (out < min_out) { min_out = out; } @@ -308,16 +313,19 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = out; } - /* scale down controls if some outputs are negative, keep total thrust */ + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ if (min_out < 0.0) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = scale_in * (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale) + - thrust; + outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += yaw * _rotors[i].yaw_scale; } } From bc3ca8db5646cf2a2e235cf7ca3bd62335e062c2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 14:26:32 +0200 Subject: [PATCH 014/105] Multirotor mixer: yaw limiting bug fixed --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 672784f468..8568f9e60f 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -299,7 +299,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { - yaw = out / _rotors[i].yaw_scale; + yaw = -out / _rotors[i].yaw_scale; } /* calculate min and max output values */ From d9a7e528b056556112c74d13a86f30bdab88f635 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 15:44:56 +0200 Subject: [PATCH 015/105] Multirotor mixer: idle_speed (aka deadband) fixed --- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 12 ++++-------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1c889a8119..fa9d28b74c 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -516,7 +516,7 @@ private: float _roll_scale; float _pitch_scale; float _yaw_scale; - float _deadband; + float _idle_speed; unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8568f9e60f..1ca0a21e97 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -176,12 +176,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, float roll_scale, float pitch_scale, float yaw_scale, - float deadband) : + float idle_speed) : Mixer(control_cb, cb_handle), _roll_scale(roll_scale), _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), - _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _rotor_count(_config_rotor_count[geometry]), _rotors(_config_index[geometry]) { @@ -338,15 +338,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) scale_out = 1.0f; } + /* scale outputs to range _idle_speed..1 */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = -1.0f + (outputs[i] * 2 * scale_out); + outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out); } - /* ensure outputs are out of the deadband */ - for (unsigned i = 0; i < _rotor_count; i++) - if (outputs[i] < _deadband) - outputs[i] = _deadband; - return _rotor_count; } From 692e8f84a93a932986004d896554a70380ea11e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 May 2014 22:12:07 +0200 Subject: [PATCH 016/105] commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic --- src/modules/commander/commander.cpp | 21 ++++++++++++++++++- .../position_estimator_inav_main.c | 21 +++++++++++++++---- src/modules/sdlog2/sdlog2.c | 10 +++++++-- src/modules/sdlog2/sdlog2_messages.h | 7 ++++--- .../uORB/topics/vehicle_local_position.h | 2 ++ 5 files changed, 51 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd0..e4ae357d70 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1012,7 +1012,26 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); + /* hysteresis for EPH */ + bool local_eph_good; + + if (status.condition_global_position_valid) { + if (local_position.eph > eph_epv_threshold * 2.0f) { + local_eph_good = false; + + } else { + local_eph_good = true; + } + + } else { + if (local_position.eph < eph_epv_threshold) { + local_eph_good = true; + + } else { + local_eph_good = false; + } + } + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d8d0ff37d3..fdc3233e03 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float eph = 1.0; + float epv = 1.0; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_valid = true; + eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + } else { w_flow = 0.0f; flow_valid = false; @@ -673,6 +678,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + eph = fminf(eph, gps.eph_m); + epv = fminf(epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -712,6 +720,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; + /* increase EPH/EPV on each step */ + eph *= 1.0 + dt; + epv += 0.005 * 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; @@ -723,7 +735,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + bool can_estimate_xy = eph < max_eph_epv * 1.5; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -922,6 +934,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.landed = landed; 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; @@ -950,9 +964,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; - // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = eph; + global_pos.epv = epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39f433eb53..70ce768068 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1119,10 +1119,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; 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.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | + (buf.local_pos.z_valid ? 2 : 0) | + (buf.local_pos.v_xy_valid ? 4 : 0) | + (buf.local_pos.v_z_valid ? 8 : 0) | + (buf.local_pos.xy_global ? 16 : 0) | + (buf.local_pos.z_global ? 32 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + log_msg.body.log_LPOS.eph = buf.local_pos.eph; + log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0c61886579..90025b9ff6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,10 +109,11 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - uint8_t xy_flags; - uint8_t z_flags; + uint8_t pos_flags; uint8_t landed; uint8_t ground_dist_flags; + float eph; + float epv; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -360,7 +361,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index a15303ea48..5d39c897d4 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -83,6 +83,8 @@ struct vehicle_local_position_s { float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ + float eph; + float epv; }; /** From f52edc4cdee0aec037d5def8ee805bb1904bb6f9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 09:18:54 +0200 Subject: [PATCH 017/105] ekf_att_pos_estimator: fixed files names and perf counters names --- ...r_main.cpp => ekf_att_pos_estimator_main.cpp} | 16 ++++++++-------- ...r_params.c => ekf_att_pos_estimator_params.c} | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_main.cpp => ekf_att_pos_estimator_main.cpp} (98%) rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_params.c => ekf_att_pos_estimator_params.c} (99%) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp similarity index 98% rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 907f4c2e1d..7a71894ed8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file fw_att_pos_estimator_main.cpp + * @file ekf_att_pos_estimator_main.cpp * Implementation of the attitude and position estimator. * * @author Paul Riseborough @@ -336,13 +336,13 @@ FixedwingEstimator::FixedwingEstimator() : _baro_gps_offset(0.0f), /* performance counters */ - _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")), - _perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")), - _perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")), - _perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")), - _perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")), - _perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")), + _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")), + _perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")), + _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")), /* states */ _initialized(false), diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c similarity index 99% rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index d2c6e1f15e..1d9ae46236 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file fw_att_pos_estimator_params.c + * @file ekf_att_pos_estimator_params.c * * Parameters defined by the attitude and position estimator task * From 98fcec243bac42507aa4469bd79831f847e2e846 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 09:21:47 +0200 Subject: [PATCH 018/105] ekf_att_pos_estimator: module.mk fixed --- src/modules/ekf_att_pos_estimator/module.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 30955d0ddc..6fefec2c2a 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -32,11 +32,11 @@ ############################################################################ # -# Main Attitude and Position Estimator for Fixed Wing Aircraft +# Main EKF Attitude and Position Estimator # MODULE_COMMAND = ekf_att_pos_estimator -SRCS = fw_att_pos_estimator_main.cpp \ - fw_att_pos_estimator_params.c \ +SRCS = ekf_att_pos_estimator_main.cpp \ + ekf_att_pos_estimator_params.c \ estimator.cpp From 32ae2dd1d094f4554f5acdad8fb76ed9eb3ba1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 11:00:14 +0200 Subject: [PATCH 019/105] commander: missed 'break' in 'switch' added --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd0..504696ff94 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1710,6 +1710,7 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; From e72a7a7dd78cde4a93d42a53328553e71560fc27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:14 +0200 Subject: [PATCH 020/105] fw att: robustify main loop against non finite numbers and limit error output rate --- .../fw_att_control/fw_att_control_main.cpp | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1f3e9f098a..cafce4417d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main() 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); @@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - warnx("Did not get a valid R\n"); + if (loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } } /* Run attitude controllers */ @@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + _roll_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("roll_u %.4f", roll_u); + } } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { - 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); + _pitch_ctrl.reset_integrator(); + if (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(_att.roll, _att.pitch, @@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", (double)yaw_u); + _yaw_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double)yaw_u); + } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", (double)throttle_sp); + if (loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double)throttle_sp); + } } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + if (loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + } } /* @@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main() } + loop_counter++; perf_end(_loop_perf); } From 7c165689ce0d33a65554f62a3438c2da30404642 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:37 +0200 Subject: [PATCH 021/105] fw att: pitch ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 9584924ccc..a3f5199b1a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() : float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { - + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + warnx("not controlling pitch"); + return _rate_setpoint; + } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -123,6 +127,13 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); From 52596be98c77497d67615435fdbd8e403cae618f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:50 +0200 Subject: [PATCH 022/105] fw att: roll ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 2e86c72dc1..6ad00049d9 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() : float ECL_RollController::control_attitude(float roll_setpoint, float roll) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll_setpoint) && isfinite(roll))) { + return _rate_setpoint; + } /* Calculate error */ float roll_error = roll_setpoint - roll; @@ -86,6 +90,13 @@ float ECL_RollController::control_bodyrate(float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && + isfinite(airspeed_min) && isfinite(airspeed_max) && + isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); @@ -122,8 +133,8 @@ float ECL_RollController::control_bodyrate(float pitch, float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ + * anti-windup: do not allow integrator to increase if actuator is at limit + */ if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); From 9a35bac2adc1e803dea7cdb6a2db277f111724e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:16:10 +0200 Subject: [PATCH 023/105] fw att: yaw ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 255776765b..d43e0314e9 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,6 +66,12 @@ float ECL_YawController::control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && + isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && + isfinite(pitch_rate_setpoint))) { + return _rate_setpoint; + } // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; @@ -103,6 +109,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float pitch_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); From ce62f073796f5956b32eaa8eb8d4a371821077de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 May 2014 21:06:32 +0200 Subject: [PATCH 024/105] Fix EPH / EPV conversion for MediaTek units --- src/drivers/gps/mtk.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c90ecbe281..60a98134db 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -253,8 +253,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->eph_m = packet.hdop / 100.0f; // from cm to m + _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; From f0630547aa6d391fbb056e450d2344bd888721e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 May 2014 21:07:19 +0200 Subject: [PATCH 025/105] MTK: Bail out correctly --- src/drivers/gps/mtk.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 60a98134db..680f00d97f 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -249,6 +249,12 @@ MTK::handle_message(gps_mtk_packet_t &packet) warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; + + // Indicate this data is not usable and bail out + _gps_position->eph_m = 1000.0f; + _gps_position->epv_m = 1000.0f; + _gps_position->fix_type = 0; + return; } _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm From c945693054a15db03414b0446372c7a457fa743a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 May 2014 16:33:15 +1000 Subject: [PATCH 026/105] ets airspeed: Support raw field --- src/drivers/ets_airspeed/ets_airspeed.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 146a06e7cf..2de7063ea7 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -154,8 +154,9 @@ ETSAirspeed::collect() return ret; } - uint16_t diff_pres_pa = val[1] << 8 | val[0]; - if (diff_pres_pa == 0) { + uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; + uint16_t diff_pres_pa; + if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the // caller could end up using this value as part of an @@ -165,10 +166,10 @@ ETSAirspeed::collect() return -1; } - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; } else { - diff_pres_pa -= _diff_pres_offset; + diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; } // Track maximum differential pressure measured (so we can work out top speed). @@ -183,6 +184,7 @@ ETSAirspeed::collect() // XXX we may want to smooth out the readings to remove noise. report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; From 56a8f3de0a518d189707ef0da062325ab797f4a9 Mon Sep 17 00:00:00 2001 From: John Hiesey Date: Tue, 20 May 2014 00:47:48 -0700 Subject: [PATCH 027/105] Add mixer config for hexa coax frame --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 -- src/modules/systemlib/mixer/mixer.h | 1 + src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++++++ src/modules/systemlib/mixer/multi_tables | 15 ++++++++++++--- 4 files changed, 26 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index df2e609bc8..daa04a4de8 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -1,7 +1,5 @@ #!nsh # -# UNTESTED UNTESTED! -# # Generic 10" Hexa coaxial geometry # # Lorenz Meier diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index fa9d28b74c..225570fa40 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -447,6 +447,7 @@ public: QUAD_WIDE, /**< quad in wide configuration */ HEX_X, /**< hex in X configuration */ HEX_PLUS, /**< hex in + configuration */ + HEX_COX, OCTA_X, OCTA_PLUS, OCTA_COX, diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 1ca0a21e97..4ad21d8188 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -115,6 +115,14 @@ const MultirotorMixer::Rotor _config_hex_plus[] = { { 0.866025, 0.500000, 1.00 }, { -0.866025, -0.500000, -1.00 }, }; +const MultirotorMixer::Rotor _config_hex_cox[] = { + { -0.866025, 0.500000, -1.00 }, + { -0.866025, 0.500000, 1.00 }, + { -0.000000, -1.000000, -1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 0.866025, 0.500000, -1.00 }, + { 0.866025, 0.500000, 1.00 }, +}; const MultirotorMixer::Rotor _config_octa_x[] = { { -0.382683, 0.923880, -1.00 }, { 0.382683, -0.923880, -1.00 }, @@ -152,6 +160,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_wide[0], &_config_hex_x[0], &_config_hex_plus[0], + &_config_hex_cox[0], &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], @@ -163,6 +172,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_wide */ 6, /* hex_x */ 6, /* hex_plus */ + 6, /* hex_cox */ 8, /* octa_x */ 8, /* octa_plus */ 8, /* octa_cox */ @@ -252,6 +262,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "6x")) { geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "6c")) { + geometry = MultirotorMixer::HEX_COX; + } else if (!strcmp(geomname, "8+")) { geometry = MultirotorMixer::OCTA_PLUS; diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 050bf2f47f..b5698036e1 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -52,6 +52,15 @@ set hex_plus { 120 CW } +set hex_cox { + 60 CW + 60 CCW + 180 CW + 180 CCW + -60 CW + -60 CCW +} + set octa_x { 22.5 CW -157.5 CW @@ -85,7 +94,7 @@ set octa_cox { -135 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox} +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} @@ -104,13 +113,13 @@ foreach table $tables { puts "};" } -puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {" foreach table $tables { puts [format "\t&_config_%s\[0\]," $table] } puts "};" -puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {" foreach table $tables { upvar #0 $table angles puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] From 4a666d094d016527bcb342e6209fb3ad40f0ed58 Mon Sep 17 00:00:00 2001 From: John Hiesey Date: Tue, 20 May 2014 01:25:52 -0700 Subject: [PATCH 028/105] frsky_telemetry: fix gps data format Eliminate inadvertent rounding and generate degrees/minutes instead of degrees/minutes/seconds output per frsky docs --- src/drivers/frsky_telemetry/frsky_data.c | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 18de3f4dac..dd9b90fb33 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -194,17 +194,12 @@ void frsky_send_frame1(int uart) } /** - * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + * Formats the decimal latitude/longitude to the required degrees/minutes. */ static float frsky_format_gps(float dec) { - float dms_deg = (int) dec; - float dec_deg = dec - dms_deg; - float dms_min = (int) (dec_deg * 60); - float dec_min = (dec_deg * 60) - dms_min; - float dms_sec = dec_min * 60; - - return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); + float dm_deg = (int) dec; + return (dm_deg * 100.0f) + (dec - dm_deg) * 60; } /** @@ -232,9 +227,9 @@ void frsky_send_frame2(int uart) struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = frsky_format_gps(abs(global_pos.lat)); + lat = frsky_format_gps(fabsf(global_pos.lat)); lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = frsky_format_gps(abs(global_pos.lon)); + lon = frsky_format_gps(fabsf(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 25.0f / 46.0f; From d8ef397b07280a2631076c2f92b950d64684a8ed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 16:40:44 +0200 Subject: [PATCH 029/105] mc_att_control: reset yaw setpoint after ACRO --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 77c7c61e9b..20e016da34 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -803,6 +803,9 @@ MulticopterAttitudeControl::task_main() _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; + /* 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); From 559bfbb11cdac499cda8070c32e2b7e79b3b883e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 16:42:11 +0200 Subject: [PATCH 030/105] commander: allow disarm in ACRO mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8336bcf329..45ef79d7c5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1192,7 +1192,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { From fb801b6faecd77fe2aac54d3389cacf73993ccc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 21:46:42 +0200 Subject: [PATCH 031/105] mavlink: minor fix --- src/modules/mavlink/mavlink_messages.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2837c16b80..933478f560 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -140,7 +140,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set } else if (status->main_state == MAIN_STATE_ACRO) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } } else { From 9024d76e7c09bdc4296aa991c40b7048426f44eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 10:20:18 +0200 Subject: [PATCH 032/105] Fixed up SNR logging to include maximum 32 satellites (which is plenty even for future, not yet deployed global positioning systems) --- src/modules/sdlog2/sdlog2.c | 39 +++++++++++++++++++++------- src/modules/sdlog2/sdlog2_messages.h | 30 +++++++++++++++------ 2 files changed, 51 insertions(+), 18 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 70ce768068..15d50d5d7e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -834,8 +834,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; - struct log_GSN0_s log_GSN0; - struct log_GSN1_s log_GSN1; + struct log_GS0A_s log_GS0A; + struct log_GS0B_s log_GS0B; + struct log_GS1A_s log_GS1A; + struct log_GS1B_s log_GS1B; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -969,7 +971,7 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } - /* --- GPS POSITION --- */ + /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; @@ -986,16 +988,33 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); /* log the SNR of each satellite for a detailed view of signal quality */ - log_msg.msg_type = LOG_GSN0_MSG; - /* pick the smaller number so we do not overflow any of the arrays */ unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); - unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]); - unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr; + unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); - for (unsigned i = 0; i < sat_max_snr; i++) { - log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + log_msg.msg_type = LOG_GS0A_MSG; + memset(log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); + /* fill set A */ + unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < max_sats_a; i++) { + log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + } + LOGBUFFER_WRITE_AND_COUNT(GS0A); + + /* do we need a 2nd set? */ + if (gps_msg_max_snr > log_max_snr) { + log_msg.msg_type = LOG_GS0B_MSG; + memset(log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B - deduct the count we already have taken care of */ + gps_msg_max_snr -= log_max_snr; + unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < max_sats_b; i++) { + /* count from zero, but obey offset of log_max_snr consumed units */ + log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i]; + } + LOGBUFFER_WRITE_AND_COUNT(GS0B); } - LOGBUFFER_WRITE_AND_COUNT(GSN0); } /* --- SENSOR COMBINED --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90025b9ff6..85ef4da39f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -318,15 +318,27 @@ struct log_VICN_s { float yaw; }; -/* --- GSN0 - GPS SNR #0 --- */ -#define LOG_GSN0_MSG 26 -struct log_GSN0_s { +/* --- GS0A - GPS SNR #0, SAT GROUP A --- */ +#define LOG_GS0A_MSG 26 +struct log_GS0A_s { uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ }; -/* --- GSN1 - GPS SNR #1 --- */ -#define LOG_GSN1_MSG 27 -struct log_GSN1_s { +/* --- GS0B - GPS SNR #0, SAT GROUP B --- */ +#define LOG_GS0B_MSG 27 +struct log_GS0B_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + +/* --- GS1A - GPS SNR #1, SAT GROUP A --- */ +#define LOG_GS1A_MSG 28 +struct log_GS1A_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + +/* --- GS1B - GPS SNR #1, SAT GROUP B --- */ +#define LOG_GS1B_MSG 29 +struct log_GS1B_s { uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ }; @@ -381,8 +393,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 66f57f577b043cd263d16425fe154c26893d88fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 May 2014 11:28:18 +0200 Subject: [PATCH 033/105] ardrone interface: reduce stack size of start handler --- src/drivers/ardrone_interface/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk index 058bd1397d..d8e6c76c62 100644 --- a/src/drivers/ardrone_interface/module.mk +++ b/src/drivers/ardrone_interface/module.mk @@ -38,3 +38,4 @@ MODULE_COMMAND = ardrone_interface SRCS = ardrone_interface.c \ ardrone_motor_control.c +MODULE_STACKSIZE = 1200 From 981d61626890e979251a7fb8e0ddbe6678220e2c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 May 2014 11:29:14 +0200 Subject: [PATCH 034/105] ardrone interface: reduce stack size Conflicts: src/drivers/ardrone_interface/ardrone_interface.c --- src/drivers/ardrone_interface/ardrone_interface.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index b88f61ce80..e5bb772b3c 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -119,7 +119,7 @@ int ardrone_interface_main(int argc, char *argv[]) ardrone_interface_task = task_spawn_cmd("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, - 2048, + 1100, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From aa2b125a67c4b4d64b8c4b5bed662cdac1e5f1c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:19:39 +0200 Subject: [PATCH 035/105] perf: Allow printing to arbritrary fds --- src/modules/systemlib/perf_counter.c | 16 +++++++++++----- src/modules/systemlib/perf_counter.h | 16 +++++++++++++--- src/systemcmds/perf/perf.c | 2 +- src/systemcmds/tests/tests_main.c | 2 +- 4 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index b4ca0ed3ec..22182e39e8 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -281,13 +281,19 @@ perf_reset(perf_counter_t handle) void perf_print_counter(perf_counter_t handle) +{ + perf_print_counter_fd(0, handle); +} + +void +perf_print_counter_fd(int fd, perf_counter_t handle) { if (handle == NULL) return; switch (handle->type) { case PC_COUNT: - printf("%s: %llu events\n", + dprintf(fd, "%s: %llu events\n", handle->name, ((struct perf_ctr_count *)handle)->event_count); break; @@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, @@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, @@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle) } void -perf_print_all(void) +perf_print_all(int fd) { perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); while (handle != NULL) { - perf_print_counter(handle); + perf_print_counter_fd(fd, handle); handle = (perf_counter_t)sq_next(&handle->link); } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index d8f69fdbf7..6835ee4a2f 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -121,16 +121,26 @@ __EXPORT extern void perf_cancel(perf_counter_t handle); __EXPORT extern void perf_reset(perf_counter_t handle); /** - * Print one performance counter. + * Print one performance counter to stdout * * @param handle The counter to print. */ __EXPORT extern void perf_print_counter(perf_counter_t handle); /** - * Print all of the performance counters. + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. */ -__EXPORT extern void perf_print_all(void); +__EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +__EXPORT extern void perf_print_all(int fd); /** * Reset all of the performance counters. diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b69ea597b1..b08a2e3b75 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -73,7 +73,7 @@ int perf_main(int argc, char *argv[]) return -1; } - perf_print_all(); + perf_print_all(0 /* stdout */); fflush(stdout); return 0; } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index fe22f61773..e3f26924ff 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -235,7 +235,7 @@ test_perf(int argc, char *argv[]) printf("perf: expect count of 1\n"); perf_print_counter(ec); printf("perf: expect at least two counters\n"); - perf_print_all(); + perf_print_all(0); perf_free(cc); perf_free(ec); From 46431f12579b9344ec1c77c46087c23fefa195f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:19:56 +0200 Subject: [PATCH 036/105] Log perf counters --- src/modules/sdlog2/sdlog2.c | 78 ++++++++++++++++++++++++++++++++++--- 1 file changed, 72 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 15d50d5d7e..fa3e86e7f4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -89,6 +89,7 @@ #include #include +#include #include #include @@ -218,6 +219,8 @@ static int create_log_dir(void); */ static int open_log_file(void); +static int open_perf_file(const char* str); + static void sdlog2_usage(const char *reason) { @@ -349,8 +352,8 @@ int create_log_dir() int open_log_file() { /* string to hold the path to the log */ - char log_file_name[16] = ""; - char log_file_path[48] = ""; + char log_file_name[32] = ""; + char log_file_path[64] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ @@ -378,7 +381,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already", MAX_NO_LOGFILE); + mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -387,7 +390,58 @@ int open_log_file() if (fd < 0) { warn("failed opening log: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + + } else { + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + } + + return fd; +} + +int open_perf_file(const char* str) +{ + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); + + } else { + unsigned file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); + + if (!file_exist(log_file_path)) { + break; + } + + file_number++; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + return -1; + } + } + + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { warnx("log file: %s", log_file_name); @@ -529,6 +583,12 @@ void sdlog2_start_log() errx(1, "error creating logwriter thread"); } + /* write all performance counters */ + int perf_fd = open_perf_file("preflight"); + dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); + perf_print_all(perf_fd); + close(perf_fd); + logging_enabled = true; } @@ -556,6 +616,12 @@ void sdlog2_stop_log() logwriter_pthread = 0; pthread_attr_destroy(&logwriter_attr); + /* write all performance counters */ + int perf_fd = open_perf_file("postflight"); + dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); + perf_print_all(perf_fd); + close(perf_fd); + sdlog2_status(); } @@ -992,7 +1058,7 @@ int sdlog2_thread_main(int argc, char *argv[]) unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; - memset(log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); + memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; @@ -1004,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* do we need a 2nd set? */ if (gps_msg_max_snr > log_max_snr) { log_msg.msg_type = LOG_GS0B_MSG; - memset(log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); /* fill set B - deduct the count we already have taken care of */ gps_msg_max_snr -= log_max_snr; unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; From aa312f96f8d682c85b422ef8c5fbc89b9391712e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:20:48 +0200 Subject: [PATCH 037/105] drivers: Fix compile warnings and non-standard performance counter names --- src/drivers/mpu6000/mpu6000.cpp | 1 - src/drivers/ms5611/ms5611.cpp | 4 ++-- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 7 ++----- src/drivers/stm32/adc/adc.cpp | 2 +- 5 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ac75682c47..321fdd1733 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1380,7 +1380,6 @@ MPU6000_gyro::init() _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); -out: return ret; } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 3fe1b0abcb..1ce93aeeaf 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -753,8 +753,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); - printf("P: %.3f\n", _P); - printf("T: %.3f\n", _T); + printf("P: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 43318ca849..3b210ac593 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index a0cf98340a..9109af14f7 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -254,9 +254,6 @@ SF0X::~SF0X() int SF0X::init() { - int ret = ERROR; - unsigned i = 0; - /* do regular cdev init */ if (CDev::init() != OK) { goto out; @@ -594,7 +591,7 @@ SF0X::collect() valid = false; /* wipe out partially read content from last cycle(s), check for dot */ - for (int i = 0; i < (lend - 2); i++) { + for (unsigned i = 0; i < (lend - 2); i++) { if (_linebuf[i] == '\n') { char buf[sizeof(_linebuf)]; memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); @@ -795,7 +792,7 @@ const int ERROR = -1; SF0X *g_dev; -void start(); +void start(const char *port); void stop(); void test(); void reset(); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index de13b89697..aa0dca60c8 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -145,7 +145,7 @@ private: ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), - _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr), _to_system_power(0) From c60561b705ddb557ce9b50cc3e41f36018708ef4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:21:47 +0200 Subject: [PATCH 038/105] mavlink: Compile warning fixes --- src/modules/mavlink/mavlink_main.cpp | 15 +++++---------- src/modules/mavlink/mavlink_main.h | 4 +--- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- src/modules/mavlink/mavlink_orb_subscription.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/mavlink/mavlink_stream.cpp | 4 ++++ src/modules/mavlink/mavlink_stream.h | 6 +++++- 7 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca71..28dd97fca1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -149,10 +149,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - } - - /* no valid instance, bail */ - if (!instance) { + default: return; } @@ -211,9 +208,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - next(nullptr), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), + next(nullptr), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -234,7 +231,6 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), - /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { @@ -2030,14 +2026,14 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, _device_name); + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); } delete _subscribe_to_stream; @@ -2243,7 +2239,6 @@ Mavlink::stream(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; - int ch; argc -= 2; argv += 2; @@ -2280,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) i++; } - if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c7a7d32f84..25c0da8206 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -221,8 +221,6 @@ private: int _mavlink_fd; bool _task_running; - perf_counter_t _loop_perf; /**< loop performance counter */ - /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ @@ -282,7 +280,7 @@ private: pthread_mutex_t _message_buffer_mutex; - + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Send one parameter. diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b4..21d5219d34 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const orb_id_t +orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5c6543e813..8c09772c8e 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -63,7 +63,7 @@ public: */ bool is_published(); void *get_data(); - const orb_id_t get_topic(); + orb_id_t get_topic(); private: const orb_id_t _topic; /*< topic metadata */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a3..53769e0cfe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context) rcv->receive_thread(NULL); delete rcv; + + return nullptr; } pthread_t diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index bb19d7e336..5ec30bd330 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t) /* interval expired, send message */ send(t); _last_sent = (t / _interval) * _interval; + + return 0; } + + return -1; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index def40d9adc..2979d20dee 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -63,9 +63,13 @@ public: MavlinkStream *next; MavlinkStream(); - ~MavlinkStream(); + virtual ~MavlinkStream(); void set_interval(const unsigned int interval); void set_channel(mavlink_channel_t channel); + + /** + * @return 0 if updated / sent, -1 if unchanged + */ int update(const hrt_abstime t); virtual MavlinkStream *new_instance() = 0; virtual void subscribe(Mavlink *mavlink) = 0; From 328fc04d29f055df8cdbd2abc8313fb29eb45148 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:22:01 +0200 Subject: [PATCH 039/105] apps: Compile warning fixes --- .../attitude_estimator_ekf_main.cpp | 14 +++----------- .../ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 7 insertions(+), 15 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 2ec889efe8..66a949af15 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -266,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; @@ -274,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; @@ -287,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* initialize parameter handles */ parameters_init(&ekf_param_handles); - uint64_t start_time = hrt_absolute_time(); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; @@ -382,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* 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_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -393,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* 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_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -445,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* 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_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -498,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7a71894ed8..0a6d3fa8fe 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1067,7 +1067,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1f3e9f098a..6943239e58 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -304,8 +304,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _actuators_0_pub(-1), _attitude_sp_pub(-1), + _actuators_0_pub(-1), _actuators_1_pub(-1), /* performance counters */ @@ -773,7 +773,7 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + warnx("roll_u %.4f", (double)roll_u); } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9cbc26efe6..5b877cd77d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -418,8 +418,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), - last_manual(false), usePreTakeoffThrust(false), + last_manual(false), flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), From b9b81beb17eb449921f11f46bc419056dce03852 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 May 2014 21:49:00 +0200 Subject: [PATCH 040/105] fw att: add performance counter --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 8 ++++++++ .../ecl/attitude_fw/ecl_pitch_controller.h | 4 ++++ .../ecl/attitude_fw/ecl_roll_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 4 ++++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 ++++ .../fw_att_control/fw_att_control_main.cpp | 19 +++++++++++++++++-- 7 files changed, 53 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a3f5199b1a..0a909d02f5 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -63,12 +63,19 @@ ECL_PitchController::ECL_PitchController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); +} + +ECL_PitchController::~ECL_PitchController() +{ + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -131,6 +138,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 30a82a86a1..39b9f9d03c 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); + ~ECL_PitchController(); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); @@ -126,6 +129,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6ad00049d9..82903ef5aa 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -61,12 +61,19 @@ ECL_RollController::ECL_RollController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); +} + +ECL_RollController::~ECL_RollController() +{ + perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -94,6 +101,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 92c64b95f5..0799dbe03b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); + ~ECL_RollController(); + float control_attitude(float roll_setpoint, float roll); float control_bodyrate(float pitch, @@ -117,6 +120,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d43e0314e9..e53ffc6448 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -60,6 +60,12 @@ ECL_YawController::ECL_YawController() : _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f) { + perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); +} + +ECL_YawController::~ECL_YawController() +{ + perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -70,6 +76,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -113,6 +120,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 03f3202d03..a360c14b89 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,12 +50,15 @@ #include #include +#include class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); + ~ECL_YawController(); + float control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint); @@ -118,6 +121,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; + perf_counter_t _nonfinite_input_perf; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 59854e734e..178b590ae5 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -134,6 +134,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ @@ -310,6 +312,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false) { @@ -387,6 +391,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() } while (_control_task != -1); } + perf_free(_loop_perf); + perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_output_perf); + att_control::g_control = nullptr; } @@ -674,10 +682,12 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (!isfinite(_airspeed.true_airspeed_m_s) || + 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 { airspeed = _airspeed.true_airspeed_m_s; } @@ -778,6 +788,8 @@ FixedwingAttitudeControl::task_main() _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 (loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } @@ -790,6 +802,7 @@ FixedwingAttitudeControl::task_main() _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 (loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," @@ -813,6 +826,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -826,6 +840,7 @@ FixedwingAttitudeControl::task_main() } } } else { + perf_count(_nonfinite_input_perf); if (loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } From 7d7aaca18c9227e03875dc65a4f0012be5c1e7cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 May 2014 08:03:58 +0200 Subject: [PATCH 041/105] Introduced extended logging mode to shield general userbase from developer logging options --- src/modules/sdlog2/sdlog2.c | 64 +++++++++++++++++++++---------------- 1 file changed, 37 insertions(+), 27 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fa3e86e7f4..ff68b5ad6a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -119,6 +119,8 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; +static bool _extended_logging = false; + static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -228,12 +230,13 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t -x\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-t\tUse date/time for naming log directories and files\n" + "\t-x\tExtended logging"); } /** @@ -745,7 +748,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -781,6 +784,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_name_timestamp = true; break; + case 'x': + _extended_logging = true; + break; + case '?': if (optopt == 'c') { warnx("option -%c requires an argument", optopt); @@ -1053,33 +1060,35 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; LOGBUFFER_WRITE_AND_COUNT(GPS); - /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); - unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); + if (_extended_logging) { + /* log the SNR of each satellite for a detailed view of signal quality */ + unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); - log_msg.msg_type = LOG_GS0A_MSG; - memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - /* fill set A */ - unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; + log_msg.msg_type = LOG_GS0A_MSG; + memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); + /* fill set A */ + unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - for (unsigned i = 0; i < max_sats_a; i++) { - log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; - } - LOGBUFFER_WRITE_AND_COUNT(GS0A); - - /* do we need a 2nd set? */ - if (gps_msg_max_snr > log_max_snr) { - log_msg.msg_type = LOG_GS0B_MSG; - memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); - /* fill set B - deduct the count we already have taken care of */ - gps_msg_max_snr -= log_max_snr; - unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - - for (unsigned i = 0; i < max_sats_b; i++) { - /* count from zero, but obey offset of log_max_snr consumed units */ - log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i]; + for (unsigned i = 0; i < max_sats_a; i++) { + log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + } + LOGBUFFER_WRITE_AND_COUNT(GS0A); + + /* do we need a 2nd set? */ + if (gps_msg_max_snr > log_max_snr) { + log_msg.msg_type = LOG_GS0B_MSG; + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B - deduct the count we already have taken care of */ + gps_msg_max_snr -= log_max_snr; + unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < max_sats_b; i++) { + /* count from zero, but obey offset of log_max_snr consumed units */ + log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i]; + } + LOGBUFFER_WRITE_AND_COUNT(GS0B); } - LOGBUFFER_WRITE_AND_COUNT(GS0B); } } @@ -1425,6 +1434,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } From 04a25fcf1402feab10ede27fe0fe710b85b95941 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 May 2014 13:58:12 +0200 Subject: [PATCH 042/105] makefiles: check for correct arm-none-eabi-gcc version --- makefiles/toolchain_gnu-arm-eabi.mk | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b519e0e7a1..d4689c63ea 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -48,6 +48,16 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump +# Check if the right version of the toolchain is available +# +CROSSDEV_VER_SUPPORTED = 4.7.4 +CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) + +ifneq ($(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED)) +endif + + # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup MAXOPTIMIZATION ?= -O3 @@ -76,7 +86,7 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = # Pick the right set of flags for the architecture. # @@ -265,7 +275,7 @@ define SYM_TO_BIN $(Q) $(OBJCOPY) -O binary $1 $2 endef -# Take the raw binary $1 and make it into an object file $2. +# Take the raw binary $1 and make it into an object file $2. # The symbol $3 points to the beginning of the file, and $3_len # gives its length. # From 635a7533b487389bed27e429fb6baf936f7bb3b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:04:04 +0200 Subject: [PATCH 043/105] mc mixer: additional safe limiting of mixed out --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4ad21d8188..092c0e2b04 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -351,9 +351,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) scale_out = 1.0f; } - /* scale outputs to range _idle_speed..1 */ + /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out); + outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } return _rotor_count; From 4ee647015a5fa4c2d8e324e75a9f2d749a1b6ca6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:52:22 +0200 Subject: [PATCH 044/105] navigator: takeoff fix, always reach takeoff altitude, even if first waypoint is lower --- src/modules/navigator/navigator_main.cpp | 32 ++++++++++++------------ 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079f..8eedcc90e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1477,27 +1477,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } From 95a8414895d0f93bf92b8339ad15a1b3b4d1a7f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:55:24 +0200 Subject: [PATCH 045/105] rc.mc_defaults: set default acceptance radius (NAV_ACCEPT_RAD) to 2m --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4ed..65f1e38c6b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -41,6 +41,7 @@ then param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set NAV_ACCEPT_RAD 2.0 fi set PWM_RATE 400 From 577dc879d3e6e287b03c743e7205375f13bb3081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 12:27:13 +0200 Subject: [PATCH 046/105] Removed obsolete flow control example --- .../flow_position_control_main.c | 613 ------------------ .../flow_position_control_params.c | 124 ---- .../flow_position_control_params.h | 100 --- src/examples/flow_position_control/module.mk | 41 -- 4 files changed, 878 deletions(-) delete mode 100644 src/examples/flow_position_control/flow_position_control_main.c delete mode 100644 src/examples/flow_position_control/flow_position_control_params.c delete mode 100644 src/examples/flow_position_control/flow_position_control_params.h delete mode 100644 src/examples/flow_position_control/module.mk diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c deleted file mode 100644 index 391e40ac13..0000000000 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ /dev/null @@ -1,613 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file flow_position_control.c - * - * Optical flow position controller - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flow_position_control_params.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int flow_position_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int flow_position_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn_cmd(). - */ -int flow_position_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { - printf("flow position control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("flow_position_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 4096, - flow_position_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) - { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) - { - if (thread_running) - printf("\tflow position control app is running\n"); - else - printf("\tflow position control app not started\n"); - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int -flow_position_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - thread_running = true; - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[fpc] started"); - - uint32_t counter = 0; - const float time_scale = powf(10.0f,-6.0f); - - /* structures */ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct filtered_bottom_flow_s filtered_flow; - memset(&filtered_flow, 0, sizeof(filtered_flow)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_bodyframe_speed_setpoint_s speed_sp; - memset(&speed_sp, 0, sizeof(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 armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - 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; - static bool status_changed = 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); - mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); - } - - /* 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(actuator_armed), armed_sub, &armed); - /* 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); - /* get a local copy of control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - if (control_mode.flag_control_velocity_enabled) - { - 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 - - if(status_changed == false) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); - - status_changed = true; - - /* 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; //manual.throttle; - 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 */ - //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); - if(status_changed == true) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); - - status_changed = false; - 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++; - } - else - { - /* sensors not ready waiting for first attitude msg */ - - /* polling */ - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - /* wait for a flow msg, check for exit condition every 5 s */ - int ret = poll(fds, 1, 5000); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ - mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN) - { - sensors_ready = true; - mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); - } - } - } - } - - mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); - - thread_running = false; - - close(parameter_update_sub); - close(vehicle_attitude_sub); - close(vehicle_local_position_sub); - close(armed_sub); - close(control_mode_sub); - close(manual_control_setpoint_sub); - close(speed_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - return 0; -} - diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c deleted file mode 100644 index eb14736473..0000000000 --- a/src/examples/flow_position_control/flow_position_control_params.c +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_control_params.c - */ - -#include "flow_position_control_params.h" - -/* controller parameters */ - -// Position control P gain -PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); -// Position control D / damping gain -PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); -// Altitude control P gain -PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); -// Altitude control I (integrator) gain -PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); -// Altitude control D gain -PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); -// Altitude control rate limiter -PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); -// Altitude control minimum altitude -PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); -// Altitude control maximum altitude (higher than 1.5m is untested) -PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); -// Altitude control feed forward throttle - adjust to the -// throttle position (0..1) where the copter hovers in manual flight -PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight -PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); -PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); -PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f); -PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f); -PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f); -PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f); -PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f); -PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f); - - -int parameters_init(struct flow_position_control_param_handles *h) -{ - /* PID parameters */ - h->pos_p = param_find("FPC_POS_P"); - h->pos_d = param_find("FPC_POS_D"); - h->height_p = param_find("FPC_H_P"); - h->height_i = param_find("FPC_H_I"); - h->height_d = param_find("FPC_H_D"); - h->height_rate = param_find("FPC_H_RATE"); - h->height_min = param_find("FPC_H_MIN"); - h->height_max = param_find("FPC_H_MAX"); - h->thrust_feedforward = param_find("FPC_T_FFWD"); - h->limit_speed_x = param_find("FPC_L_S_X"); - h->limit_speed_y = param_find("FPC_L_S_Y"); - h->limit_height_error = param_find("FPC_L_H_ERR"); - h->limit_thrust_int = param_find("FPC_L_TH_I"); - h->limit_thrust_upper = param_find("FPC_L_TH_U"); - h->limit_thrust_lower = param_find("FPC_L_TH_L"); - h->limit_yaw_step = param_find("FPC_L_YAW_STEP"); - h->manual_threshold = param_find("FPC_MAN_THR"); - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p) -{ - param_get(h->pos_p, &(p->pos_p)); - param_get(h->pos_d, &(p->pos_d)); - param_get(h->height_p, &(p->height_p)); - param_get(h->height_i, &(p->height_i)); - param_get(h->height_d, &(p->height_d)); - param_get(h->height_rate, &(p->height_rate)); - param_get(h->height_min, &(p->height_min)); - param_get(h->height_max, &(p->height_max)); - param_get(h->thrust_feedforward, &(p->thrust_feedforward)); - param_get(h->limit_speed_x, &(p->limit_speed_x)); - param_get(h->limit_speed_y, &(p->limit_speed_y)); - param_get(h->limit_height_error, &(p->limit_height_error)); - param_get(h->limit_thrust_int, &(p->limit_thrust_int)); - param_get(h->limit_thrust_upper, &(p->limit_thrust_upper)); - param_get(h->limit_thrust_lower, &(p->limit_thrust_lower)); - param_get(h->limit_yaw_step, &(p->limit_yaw_step)); - param_get(h->manual_threshold, &(p->manual_threshold)); - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h deleted file mode 100644 index d0c8fc722f..0000000000 --- a/src/examples/flow_position_control/flow_position_control_params.h +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_control_params.h - * - * Parameters for position controller - */ - -#include - -struct flow_position_control_params { - float pos_p; - float pos_d; - float height_p; - float height_i; - float height_d; - float height_rate; - float height_min; - float height_max; - float thrust_feedforward; - float limit_speed_x; - float limit_speed_y; - float limit_height_error; - float limit_thrust_int; - float limit_thrust_upper; - float limit_thrust_lower; - float limit_yaw_step; - float manual_threshold; - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct flow_position_control_param_handles { - param_t pos_p; - param_t pos_d; - param_t height_p; - param_t height_i; - param_t height_d; - param_t height_rate; - param_t height_min; - param_t height_max; - param_t thrust_feedforward; - param_t limit_speed_x; - param_t limit_speed_y; - param_t limit_height_error; - param_t limit_thrust_int; - param_t limit_thrust_upper; - param_t limit_thrust_lower; - param_t limit_yaw_step; - param_t manual_threshold; - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct flow_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p); diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk deleted file mode 100644 index b10dc490ac..0000000000 --- a/src/examples/flow_position_control/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build multirotor position control -# - -MODULE_COMMAND = flow_position_control - -SRCS = flow_position_control_main.c \ - flow_position_control_params.c From e710e2a2d1875e4bec2d42e7d4054b11a1195260 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 12:38:29 +0200 Subject: [PATCH 047/105] Allow any GCC 4.7 subversion --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d4689c63ea..808b635bb8 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 +CROSSDEV_VER_SUPPORTED = 4.7 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifneq ($(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED)) +ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) endif From 7255aafcb5102ec448543e3ead80a29d182dfee1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:00:22 +0200 Subject: [PATCH 048/105] Better docs in GPS topic --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 794c3f8bc7..d79e9685f3 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -85,7 +85,7 @@ struct vehicle_gps_position_s { uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; From 19989b4314c4918ce8f6e1731ba389757f95ae62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:00:42 +0200 Subject: [PATCH 049/105] Better docs on log messages --- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 85ef4da39f..3ccffd5b24 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -321,25 +321,25 @@ struct log_VICN_s { /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ #define LOG_GS0A_MSG 26 struct log_GS0A_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ #define LOG_GS0B_MSG 27 struct log_GS0B_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ #define LOG_GS1A_MSG 28 struct log_GS1A_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ #define LOG_GS1B_MSG 29 struct log_GS1B_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ From e71c386547badfb4bf2cde0e58f345ea569fc1d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:01:20 +0200 Subject: [PATCH 050/105] Always log both GPS SNR sets. Assign array IDs by PRN to get a per-satellite unique mapping --- src/modules/sdlog2/sdlog2.c | 29 +++++++++++++---------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ff68b5ad6a..873f04828f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1068,27 +1068,24 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ - unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - - for (unsigned i = 0; i < max_sats_a; i++) { - log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + for (unsigned i = 0; i < gps_msg_max_snr; i++) { + if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0A.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + } } LOGBUFFER_WRITE_AND_COUNT(GS0A); - /* do we need a 2nd set? */ - if (gps_msg_max_snr > log_max_snr) { - log_msg.msg_type = LOG_GS0B_MSG; - memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); - /* fill set B - deduct the count we already have taken care of */ - gps_msg_max_snr -= log_max_snr; - unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - - for (unsigned i = 0; i < max_sats_b; i++) { - /* count from zero, but obey offset of log_max_snr consumed units */ - log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i]; + log_msg.msg_type = LOG_GS0B_MSG; + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ + for (unsigned i = 0; i < gps_msg_max_snr; i++) { + if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0B.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; } - LOGBUFFER_WRITE_AND_COUNT(GS0B); } + LOGBUFFER_WRITE_AND_COUNT(GS0B); } } From b3d6dcb2e5a1f66c42d575f13cbc5a7eef16db27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:14:02 +0200 Subject: [PATCH 051/105] Pre-emptively increase the log buffer - after the last cleanup we got again plenty of RAM --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 517e073afb..25f31a7e04 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,6 +11,6 @@ then sdlog2 start -r 50 -a -b 4 -t else echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -a -b 16 -t + sdlog2 start -r 200 -a -b 22 -t fi fi From 98d5ed5e7357659951a067eb8e396be9c2c59a58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:27 +0200 Subject: [PATCH 052/105] sdlog2: Fix GPS sat offset math --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 873f04828f..b6b039d261 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1069,9 +1069,13 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ for (unsigned i = 0; i < gps_msg_max_snr; i++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + int satindex = buf_gps_pos.satellite_prn[i] - 1; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); @@ -1080,9 +1084,14 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); /* fill set B */ for (unsigned i = 0; i < gps_msg_max_snr; i++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + /* get second bank of satellites, thus deduct bank size from index */ + int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); From 5f752cdb7cf6c74e34b3ac79b9220c70172791e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:48 +0200 Subject: [PATCH 053/105] commander: Better audio indications on arming reject reasons --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c262000484..0463cf8f82 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[]) sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); + print_reject_arm("#audio: NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f1a353d5b3..87309b238a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Allow if HIL_STATE_ON if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); } valid_transition = false; @@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); valid_transition = false; break; From 951bd70ffcb4a1d420347534e5b999021a412be0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:57:25 +0200 Subject: [PATCH 054/105] ubx driver: Fix / shorten printfs --- src/drivers/gps/ubx.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 8a2afecb7e..f4a065c19e 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate) send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: configuration failed: RATE"); + warnx("CFG FAIL: RATE"); return 1; } @@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate) send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: configuration failed: NAV5"); + warnx("CFG FAIL: NAV5"); return 1; } @@ -194,35 +194,35 @@ UBX::configure(unsigned &baudrate) configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV POSLLH"); + warnx("MSG CFG FAIL: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); + warnx("MSG CFG FAIL: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SOL"); + warnx("MSG CFG FAIL: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV VELNED"); + warnx("MSG CFG FAIL: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SVINFO"); + warnx("MSG CFG FAIL: NAV SVINFO"); return 1; } @@ -274,7 +274,7 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ - warnx("ubx: poll error"); + warnx("poll error"); return -1; } else if (ret == 0) { @@ -310,7 +310,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { - warnx("ubx: timeout - no useful messages"); + warnx("timeout - no useful messages"); return -1; } } @@ -383,7 +383,7 @@ UBX::parse_char(uint8_t b) return 1; // message received successfully } else { - warnx("ubx: checksum wrong"); + warnx("checksum wrong"); decode_init(); return -1; } @@ -392,7 +392,7 @@ UBX::parse_char(uint8_t b) _rx_count++; } else { - warnx("ubx: buffer full"); + warnx("buffer full"); decode_init(); return -1; } From a359a1b9408c7e4827c7dcbf7c27bf76ea071662 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:24 +0200 Subject: [PATCH 055/105] Fixed audio output to provide decent user feedback --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0463cf8f82..5c0628a166 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); + /* this needs additional hints to the user - so let other messages pass and be spoken */ + mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); tune_negative(true); break; From 4f26a8b7c83636f294b500558d8f7c72ed0cc310 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:51 +0200 Subject: [PATCH 056/105] gps topic: Added snr and jamming fields --- src/modules/uORB/topics/vehicle_gps_position.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index d79e9685f3..5924a324d5 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -68,6 +68,9 @@ struct vehicle_gps_position_s { float eph_m; /**< GPS HDOP horizontal dilution of position in m */ float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + unsigned noise_per_ms; /**< */ + unsigned jamming_indicator; /**< */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ float vel_m_s; /**< GPS ground speed (m/s) */ float vel_n_m_s; /**< GPS ground speed in m/s */ From 7bf1f82f615b05bb37b8c4c45891bbde96745a4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:03:13 +0200 Subject: [PATCH 057/105] sdlog2: Logging GPS snr and jamming fields --- src/modules/sdlog2/sdlog2.c | 23 ++++++++++++++++++----- src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b6b039d261..197b652310 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -113,8 +113,8 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ -static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ +static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -641,7 +641,7 @@ int write_formats(int fd) int written = 0; /* fill message format packet for each format and write it */ - for (int i = 0; i < log_formats_num; i++) { + for (unsigned i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; written += write(fd, &log_msg_format, sizeof(log_msg_format)); } @@ -1046,6 +1046,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { + + float snr_mean = 0.0f; + + for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { + snr_mean += buf_gps_pos.satellite_snr[i]; + } + + snr_mean /= buf_gps_pos.satellites_visible; + 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; @@ -1058,6 +1067,10 @@ int sdlog2_thread_main(int argc, char *argv[]) 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; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); if (_extended_logging) { @@ -1073,7 +1086,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int satindex = buf_gps_pos.satellite_prn[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < log_max_snr)) { + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } @@ -1089,7 +1102,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < log_max_snr)) { + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3ccffd5b24..f4d88f0795 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -139,6 +139,10 @@ struct log_GPS_s { float vel_e; float vel_d; float cog; + uint8_t sats; + uint16_t snr_mean; + uint16_t noise_per_ms; + uint16_t jamming_indicator; }; /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ @@ -375,7 +379,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), From c4d79b84b4eb3b4d255706f29972cd3f31689749 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:08:33 +0200 Subject: [PATCH 058/105] bugfixes in UBX driver, emit new SNR, Jamming and noise count indices --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/gps/gps_helper.cpp | 4 ++-- src/drivers/gps/gps_helper.h | 8 ++++++-- src/drivers/gps/ubx.cpp | 25 +++++++++++++++++++++++++ src/drivers/gps/ubx.h | 28 ++++++++++++++++++++++++++-- 5 files changed, 62 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6195cd6eab..5342ccf78e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -448,10 +448,10 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, - _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2360ff39bd..3b92f1bf45 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate() return _rate_vel; } -float +void GPS_Helper::reset_update_rates() { _rate_count_vel = 0; @@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates() _interval_rate_start = hrt_absolute_time(); } -float +void GPS_Helper::store_update_rates() { _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index cfb9e0d43d..d14a95afeb 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -46,13 +46,17 @@ class GPS_Helper { public: + + GPS_Helper() {}; + virtual ~GPS_Helper() {}; + virtual int configure(unsigned &baud) = 0; virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); float get_position_update_rate(); float get_velocity_update_rate(); - float reset_update_rates(); - float store_update_rates(); + void reset_update_rates(); + void store_update_rates(); protected: uint8_t _rate_count_lat_lon; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f4a065c19e..19cf5beecf 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -226,6 +226,13 @@ UBX::configure(unsigned &baudrate) return 1; } + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } + _configured = true; return 0; } @@ -566,6 +573,24 @@ UBX::handle_message() break; } + case UBX_CLASS_MON: { + switch (_message_id) { + case UBX_MESSAGE_MON_HW: { + + struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + + _gps_position->noise_per_ms = p->noisePerMS; + _gps_position->jamming_indicator = p->jamInd; + + ret = 1; + break; + } + + default: + break; + } + } + default: break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 79a904f4a4..5cf47b60b5 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -56,6 +56,7 @@ //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_MON 0x0A /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 @@ -72,6 +73,8 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_MON_HW 0x09 + #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -210,6 +213,27 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; +struct gps_bin_mon_hw_packet { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t __reserved1; + uint32_t usedMask; + uint8_t VP[25]; + uint8_t jamInd; + uint16_t __reserved3; + uint32_t pinIrq; + uint32_t pulLH; + uint32_t pullL; +}; + + //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ // int16_t week; /**< Measurement GPS week number */ @@ -319,7 +343,7 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer +#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer class UBX : public GPS_Helper { @@ -383,7 +407,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_last; + hrt_abstime _disable_cmd_last; }; #endif /* UBX_H_ */ From fb41f752fdc486cca83f74cb6e4d5ec416bdbd89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 16:29:35 +0200 Subject: [PATCH 059/105] Fixed Easystar mixer (reported by Mark Whitehorn) --- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index db0e40fc2e..3ab2ac3d1a 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -5,4 +5,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER easystar.mix +set MIXER easystar From ffe095646c5984236731c88fabfe2b2b0cedf983 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 19:41:38 +0200 Subject: [PATCH 060/105] sdlog2: Set logging rate and extended logging based on parameters (overwrites commandline if non-standard) --- src/modules/sdlog2/sdlog2.c | 68 +++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ff68b5ad6a..81c9721121 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -98,6 +98,36 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +/** + * Logging rate. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 sets the minimum rate, + * any other value is interpreted as rate in Hertz. This + * parameter is only read out before logging starts (which + * commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_RATE, -1); + +/** + * Enable extended logging mode. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 disables extended + * logging mode, a value of 1 enables it. This + * parameter is only read out before logging starts + * (which commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_EXT, -1); + #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ @@ -814,6 +844,44 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = 0; + /* interpret logging params */ + + param_t log_rate_ph = param_find("SDLOG_RATE"); + + if (log_rate_ph != PARAM_INVALID) { + int32_t param_log_rate; + param_get(log_rate_ph, ¶m_log_rate); + + if (param_log_rate > 0) { + + /* we can't do more than ~ 500 Hz, even with a massive buffer */ + if (param_log_rate > 500) { + param_log_rate = 500; + } + + sleep_delay = 1000000 / param_log_rate; + } else if (param_log_rate == 0) { + /* we need at minimum 10 Hz to be able to see anything */ + sleep_delay = 1000000 / 10; + } + } + + param_t log_ext_ph = param_find("SDLOG_EXT"); + + if (log_ext_ph != PARAM_INVALID) { + + int32_t param_log_extended; + param_get(log_ext_ph, ¶m_log_extended); + + if (param_log_extended > 0) { + _extended_logging = true; + } else if (param_log_extended == 0) { + _extended_logging = false; + } + /* any other value means to ignore the parameter, so no else case */ + + } + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); From 69421be983c7dcd87d00df3022f8f319b0bb7365 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 19:45:29 +0200 Subject: [PATCH 061/105] px4io: Check for bad param value --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4099e55226..972f451485 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -581,8 +581,10 @@ PX4IO::init() ASSERT(_task == -1); sys_restart_param = param_find("SYS_RESTART_TYPE"); - /* Indicate restart type is unknown */ - param_set(sys_restart_param, &sys_restart_val); + if (sys_restart_param != PARAM_INVALID) { + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + } /* do regular cdev init */ ret = CDev::init(); From 287973da2837584e5f52c21ce599db913d1685c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 20:22:26 +0200 Subject: [PATCH 062/105] pwm_limit: Do proper band limiting --- src/modules/systemlib/pwm_limit/pwm_limit.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 190b315f1f..de2caf1436 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -142,6 +142,13 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ case PWM_LIMIT_STATE_ON: for (unsigned i=0; i max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } } break; default: From 542cc7d7fbf67499a172efdd52542b517ec0dc21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 20:23:05 +0200 Subject: [PATCH 063/105] fmu: Rely on pwm_limit() call for band limits, do constrain instead of altering the direction / value --- src/drivers/px4fmu/fmu.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fd69cf795b..8a4bfa18c6 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -648,11 +648,9 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + /* last resort: catch NaN and INF */ + if ((i >= outputs.noutputs) || + !isfinite(outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -664,6 +662,7 @@ PX4FMU::task_main() uint16_t pwm_limited[num_outputs]; + /* the PWM limit call takes care of out of band errors and constrains */ pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ From 0ea3e95422c4898b3ab93ba7b9b84cbdb76bad02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 20:23:33 +0200 Subject: [PATCH 064/105] px4iofirmware: Indicate with a comment that band limiting happens in pwm_limit() call --- src/modules/px4iofirmware/mixer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index ebf4f3e8e9..2f721bf1e1 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -213,6 +213,7 @@ mixer_tick(void) mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); in_mixer = false; + /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) From b4a03d8de540f71232672427491e4eb6e6df9f3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 20:38:13 +0200 Subject: [PATCH 065/105] pwm_limit: Add missing case for the arming ramp --- src/modules/systemlib/pwm_limit/pwm_limit.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index de2caf1436..ea5ba9e526 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -136,6 +136,13 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < ramp_min_pwm) { + effective_pwm[i] = ramp_min_pwm; + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } } } break; From b0d06d21099decf0537a07922ac8dea77083c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 00:13:49 +0200 Subject: [PATCH 066/105] config cmd: Eleminate warnings --- src/systemcmds/config/config.c | 42 +++++++++++++++++++++++----------- 1 file changed, 29 insertions(+), 13 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index e6d4b763be..e8ccbc149e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Author: Julian Oes + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -94,7 +92,6 @@ do_device(int argc, char *argv[]) } int fd; - int ret; fd = open(argv[0], 0); @@ -104,6 +101,8 @@ do_device(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[1], "block")) { /* disable the device publications */ @@ -132,7 +131,6 @@ static void do_gyro(int argc, char *argv[]) { int fd; - int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -142,6 +140,8 @@ do_gyro(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ @@ -173,8 +173,12 @@ do_gyro(int argc, char *argv[]) warnx("gyro self test FAILED! Check calibration:"); struct gyro_scale scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + if (ret) { + err(1, "gyro scale fail"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("gyro calibration and self test OK"); } @@ -199,7 +203,6 @@ static void do_mag(int argc, char *argv[]) { int fd; - int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -209,6 +212,8 @@ do_mag(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ @@ -240,8 +245,13 @@ do_mag(int argc, char *argv[]) warnx("mag self test FAILED! Check calibration:"); struct mag_scale scale; ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + + if (ret) { + errx(ret, "failed getting mag scale"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("mag calibration and self test OK"); } @@ -266,7 +276,6 @@ static void do_accel(int argc, char *argv[]) { int fd; - int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -276,6 +285,8 @@ do_accel(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ @@ -307,8 +318,13 @@ do_accel(int argc, char *argv[]) warnx("accel self test FAILED! Check calibration:"); struct accel_scale scale; ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + + if (ret) { + errx(ret, "failed getting mag scale"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("accel calibration and self test OK"); } From 905299884db03a680f71d5ab6301596fa50ece9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 00:15:56 +0200 Subject: [PATCH 067/105] mtd cmd: Warnings eleminated --- src/systemcmds/tests/test_mtd.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index bac9efbdb3..cdb4362bad 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -57,6 +57,8 @@ #define PARAM_FILE_NAME "/fs/mtd_params" static int check_user_abort(int fd); +static void print_fail(void); +static void print_success(void); int check_user_abort(int fd) { /* check if user wants to abort */ @@ -126,7 +128,7 @@ test_mtd(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -137,11 +139,14 @@ test_mtd(int argc, char *argv[]) int fd = open(PARAM_FILE_NAME, O_RDONLY); int rret = read(fd, read_buf, chunk_sizes[c]); close(fd); + if (rret <= 0) { + err(1, "read error"); + } fd = open(PARAM_FILE_NAME, O_WRONLY); printf("printing 2 percent of the first chunk:\n"); - for (int i = 0; i < sizeof(read_buf) / 50; i++) { + for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) { printf("%02X", read_buf[i]); } printf("\n"); @@ -171,9 +176,9 @@ test_mtd(int argc, char *argv[]) /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, chunk_sizes[c]); + int rret2 = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret2 != (int)chunk_sizes[c]) { warnx("READ ERROR!"); print_fail(); return 1; @@ -182,7 +187,7 @@ test_mtd(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d", j); print_fail(); @@ -211,7 +216,7 @@ test_mtd(int argc, char *argv[]) char ffbuf[64]; memset(ffbuf, 0xFF, sizeof(ffbuf)); int fd = open(PARAM_FILE_NAME, O_WRONLY); - for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) { int ret = write(fd, ffbuf, sizeof(ffbuf)); if (ret != sizeof(ffbuf)) { From b7d1f3f10010949241a30a5208e3e9d93306625a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 11:01:28 +0200 Subject: [PATCH 068/105] Make error reporting consistent --- src/systemcmds/config/config.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index e8ccbc149e..4a97d328cf 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -173,8 +173,9 @@ do_gyro(int argc, char *argv[]) warnx("gyro self test FAILED! Check calibration:"); struct gyro_scale scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); + if (ret) { - err(1, "gyro scale fail"); + err(1, "failed getting gyro scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); @@ -247,7 +248,7 @@ do_mag(int argc, char *argv[]) ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); if (ret) { - errx(ret, "failed getting mag scale"); + err(ret, "failed getting mag scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); @@ -320,7 +321,7 @@ do_accel(int argc, char *argv[]) ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); if (ret) { - errx(ret, "failed getting mag scale"); + err(ret, "failed getting accel scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); From 36495cdb62e21b30a5a1851ec802c9f6a40c1171 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 18:52:48 +0200 Subject: [PATCH 069/105] Allow wider param range for accel offset, lower gain\ --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 1d9ae46236..8c82dd6c14 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -226,11 +226,11 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. * Increasing this value makes the bias estimation faster and noisier. * - * @min 0.0001 + * @min 0.00001 * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f); /** * Magnetometer earth frame offsets process noise From 9a49636f6a6cfa631e8a4686e2cbb2f4e25770eb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 May 2014 13:15:17 +0200 Subject: [PATCH 070/105] position_estimator_inav: remove acceleration from state and INAV_W_XXX_ACC parameters, more NaN checks --- .../position_estimator_inav/inertial_filter.c | 15 ++-- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 68 ++++++++----------- .../position_estimator_inav_params.c | 6 -- .../position_estimator_inav_params.h | 4 -- 5 files changed, 38 insertions(+), 57 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 2f1b3c0145..a36a4688d0 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -9,15 +9,18 @@ #include "inertial_filter.h" -void inertial_filter_predict(float dt, float x[3]) +void inertial_filter_predict(float dt, float x[2], float acc) { if (isfinite(dt)) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (!isfinite(acc)) { + acc = 0.0f; + } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; + x[1] += acc * dt; } } -void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[2], int i, float w) { if (isfinite(e) && isfinite(w) && isfinite(dt)) { float ewdt = e * w * dt; @@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; - - } else if (i == 1) { - x[2] += w * ewdt; } } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 761c17097d..cdeb4cfc6c 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -8,6 +8,6 @@ #include #include -void inertial_filter_predict(float dt, float x[3]); +void inertial_filter_predict(float dt, float x[3], float acc); void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fdc3233e03..d7503e42da 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -168,15 +168,15 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -195,14 +195,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - float x_est[3] = { 0.0f, 0.0f, 0.0f }; - float y_est[3] = { 0.0f, 0.0f, 0.0f }; - float z_est[3] = { 0.0f, 0.0f, 0.0f }; + 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 eph = 1.0; float epv = 1.0; - float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + 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)); @@ -241,7 +241,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + 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] = { @@ -341,8 +341,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_offset += sensor.baro_alt_meter; - baro_init_cnt++; + if (isfinite(sensor.baro_alt_meter)) { + baro_offset += sensor.baro_alt_meter; + baro_init_cnt++; + } } else { wait_baro = false; @@ -418,19 +420,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } - corr_acc[0] = accel_NED[0] - x_est[2]; - corr_acc[1] = accel_NED[1] - y_est[2]; - corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + acc[2] += CONSTANTS_ONE_G; } else { - memset(corr_acc, 0, sizeof(corr_acc)); + memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp; @@ -628,11 +628,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* 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; - x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; z_est[0] = 0.0f; - y_est[2] = accel_NED[1]; local_pos.ref_lat = lat; local_pos.ref_lon = lon; @@ -655,10 +653,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; - x_est[2] = accel_NED[0]; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; - y_est[2] = accel_NED[1]; } /* calculate correction for position */ @@ -796,26 +792,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += att.R[j][i] * accel_bias_corr[j]; } - acc_bias[i] += c * params.w_acc_bias * dt; + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } } /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); + inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + 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); 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); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + 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); memcpy(z_est, z_est_prev, sizeof(z_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); corr_baro = 0; @@ -825,19 +821,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (can_estimate_xy) { /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); + 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(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + 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); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ - inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); - inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); - if (use_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); @@ -853,11 +846,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + 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); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 2e4f26661f..8aa08b6f2c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -42,11 +42,9 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); -PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); @@ -62,11 +60,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_acc = param_find("INAV_W_Z_ACC"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_acc = param_find("INAV_W_XY_ACC"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); @@ -85,11 +81,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_acc, &(p->w_z_acc)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_acc, &(p->w_xy_acc)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index e2be079d35..08ec996a13 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,11 +43,9 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; - float w_z_acc; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; - float w_xy_acc; float w_xy_flow; float w_gps_flow; float w_acc_bias; @@ -63,11 +61,9 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; - param_t w_z_acc; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; - param_t w_xy_acc; param_t w_xy_flow; param_t w_gps_flow; param_t w_acc_bias; From 7e7d78f50655f2f5f1291cf79a249e06f6a08017 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 21:53:21 +0200 Subject: [PATCH 071/105] ekf_att_pos_estimator: Default printing to off --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 23ecd89ac3..5db1adbb3c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -5,7 +5,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -#define EKF_DEBUG +//#define EKF_DEBUG #ifdef EKF_DEBUG #include From 00d40d095ddb4a5ad645044bed1ffdc1f977dba3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: [PATCH 072/105] mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca1..bb1ad86ef6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: From fcb890553329b4092c7dca319f5f538865734b3a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 May 2014 08:33:04 +0200 Subject: [PATCH 073/105] navigator: autocontinue and RTL autolanding fixes --- src/modules/navigator/navigator_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079f..310bdf9eab 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached() /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { @@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - set_mission_item(); + if (_mission_item.autocontinue) { + /* continue mission */ + set_mission_item(); + + } else { + /* autocontinue disabled for this item */ + request_loiter_or_ready(); + } } else { /* if no more mission items available then finish mission */ From b84cfea455ea801593b4ee04419417dcc7c4d4f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 08:46:45 +0200 Subject: [PATCH 074/105] romfs: Added back mixer readme, now gets filtered out --- ROMFS/px4fmu_common/mixers/README | 154 ++++++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/README diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README new file mode 100644 index 0000000000..60311232e8 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/README @@ -0,0 +1,154 @@ +PX4 mixer definitions +===================== + +Files in this directory implement example mixers that can be used as a basis +for customisation, or for general testing purposes. + +Mixer basics +------------ + +Mixers combine control values from various sources (control tasks, user inputs, +etc.) and produce output values suitable for controlling actuators; servos, +motors, switches and so on. + +An actuator derives its value from the combination of one or more control +values. Each of the control values is scaled according to the actuator's +configuration and then combined to produce the actuator value, which may then be +further scaled to suit the specific output type. + +Internally, all scaling is performed using floating point values. Inputs and +outputs are clamped to the range -1.0 to 1.0. + +control control control + | | | + v v v + scale scale scale + | | | + | v | + +-------> mix <------+ + | + scale + | + v + out + +Scaling +------- + +Basic scalers provide linear scaling of the input to the output. + +Each scaler allows the input value to be scaled independently for inputs +greater/less than zero. An offset can be applied to the output, and lower and +upper boundary constraints can be applied. Negative scaling factors cause the +output to be inverted (negative input produces positive output). + +Scaler pseudocode: + +if (input < 0) + output = (input * NEGATIVE_SCALE) + OFFSET +else + output = (input * POSITIVE_SCALE) + OFFSET + +if (output < LOWER_LIMIT) + output = LOWER_LIMIT +if (output > UPPER_LIMIT) + output = UPPER_LIMIT + +Syntax +------ + +Mixer definitions are text files; lines beginning with a single capital letter +followed by a colon are significant. All other lines are ignored, meaning that +explanatory text can be freely mixed with the definitions. + +Each file may define more than one mixer; the allocation of mixers to actuators +is specific to the device reading the mixer definition, and the number of +actuator outputs generated by a mixer is specific to the mixer. + +A mixer begins with a line of the form + + : + +The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a +multirotor mixer, etc. + +Null Mixer +.......... + +A null mixer consumes no controls and generates a single actuator output whose +value is always zero. Typically a null mixer is used as a placeholder in a +collection of mixers in order to achieve a specific pattern of actuator outputs. + +The null mixer definition has the form: + + Z: + +Simple Mixer +............ + +A simple mixer combines zero or more control inputs into a single actuator +output. Inputs are scaled, and the mixing function sums the result before +applying an output scaler. + +A simple mixer definition begins with: + + M: + O: <-ve scale> <+ve scale> + +If is zero, the sum is effectively zero and the mixer will +output a fixed value that is constrained by and . + +The second line defines the output scaler with scaler parameters as discussed +above. Whilst the calculations are performed as floating-point operations, the +values stored in the definition file are scaled by a factor of 10000; i.e. an +offset of -0.5 is encoded as -5000. + +The definition continues with entries describing the control +inputs and their scaling, in the form: + + S: <-ve scale> <+ve scale> + +The value identifies the control group from which the scaler will read, +and the value an offset within that group. These values are specific to +the device reading the mixer definition. + +When used to mix vehicle controls, mixer group zero is the vehicle attitude +control group, and index values zero through three are normally roll, pitch, +yaw and thrust respectively. + +The remaining fields on the line configure the control scaler with parameters as +discussed above. Whilst the calculations are performed as floating-point +operations, the values stored in the definition file are scaled by a factor of +10000; i.e. an offset of -0.5 is encoded as -5000. + +Multirotor Mixer +................ + +The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) +into a set of actuator outputs intended to drive motor speed controllers. + +The mixer definition is a single line of the form: + +R: + +The supported geometries include: + + 4x - quadrotor in X configuration + 4+ - quadrotor in + configuration + 6x - hexcopter in X configuration + 6+ - hexcopter in + configuration + 8x - octocopter in X configuration + 8+ - octocopter in + configuration + +Each of the roll, pitch and yaw scale values determine scaling of the roll, +pitch and yaw controls relative to the thrust control. Whilst the calculations +are performed as floating-point operations, the values stored in the definition +file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. + +Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the +thrust input ranges from 0.0 to 1.0. Output for each actuator is in the +range -1.0 to 1.0. + +In the case where an actuator saturates, all actuator values are rescaled so that +the saturating actuator is limited to 1.0. \ No newline at end of file From 366e9c633d7d37894ab71d46951605ebe951c062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:46:16 +0200 Subject: [PATCH 075/105] FMUv1 board prettifying --- src/drivers/boards/px4fmu-v1/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 58273f2d29..c944007a56 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,6 +86,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to @@ -98,7 +99,7 @@ __BEGIN_DECLS /* * Optional devices on IO's external port */ -#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_ACCEL_MAG 2 /* * I2C busses From e1309f239104a7493b40bebc5da3aa1e7572bd09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:51:00 +0200 Subject: [PATCH 076/105] Enable SPI4 for FMUv2 --- nuttx-configs/px4fmu-v2/include/board.h | 4 ++++ nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3bede8a1f7..3b3c6fa707 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -268,6 +268,10 @@ #define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) + /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a982040c6d..7d5e6e9df5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -235,7 +235,7 @@ CONFIG_STM32_SDIO=y CONFIG_STM32_SPI1=y CONFIG_STM32_SPI2=y # CONFIG_STM32_SPI3 is not set -# CONFIG_STM32_SPI4 is not set +CONFIG_STM32_SPI4=y # CONFIG_STM32_SPI5 is not set # CONFIG_STM32_SPI6 is not set CONFIG_STM32_SYSCFG=y From 096a4673e90240ea31fc414dbc09d3a00f24b180 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:51:18 +0200 Subject: [PATCH 077/105] Add SPI4 init bits for FMUv2 --- src/drivers/boards/px4fmu-v2/board_config.h | 5 +++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 12 +++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 35 +++++++++++++++++++++ 3 files changed, 52 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index c2de1bfba0..3ed9888e45 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -106,14 +106,19 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_EXT0 5 +#define PX4_SPIDEV_EXT1 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 71414d62c7..bf41bb1fed 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -192,6 +192,7 @@ stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct spi_dev_s *spi4; static struct sdio_dev_s *sdio; #include @@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); + spi4 = up_spiinitialize(4); + + /* Default SPI4 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi4, 10000000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); + + message("[boot] Initialized SPI port 4\n"); + #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index c66c490a7f..9033290520 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_FRAM); stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI_CS_EXT0); + stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); +#endif } __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) @@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } #endif + +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} From 2bfde311db7daa0771c7be7f165b7601bf2946ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:11 +0200 Subject: [PATCH 078/105] Fix SPI select on port 4 to use the right defines --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 9033290520..01dbd6e774 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -172,14 +172,14 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_EXT0: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); break; default: From 65344133a4a6944b71f2ca134c6c4443ff7c96cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:38 +0200 Subject: [PATCH 079/105] Count devices on SPI4 / EXT from 1 as for the other buses --- src/drivers/boards/px4fmu-v2/board_config.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 3ed9888e45..36eb7bec4e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -117,8 +117,10 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 -#define PX4_SPIDEV_EXT0 5 -#define PX4_SPIDEV_EXT1 6 + +/* External bus */ +#define PX4_SPIDEV_EXT0 1 +#define PX4_SPIDEV_EXT1 2 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 From b08a429d89cc10cb5d9dc953a218527dc20c230c Mon Sep 17 00:00:00 2001 From: Kynos Date: Wed, 28 May 2014 14:28:41 +0200 Subject: [PATCH 080/105] Updated AR.Drone parameters Flight tested May 7th, 2014. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 14786f2106..e6007db0e7 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -14,19 +14,20 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig - param set MC_ROLL_P 5.0 - param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.0 - param set MC_PITCH_P 5.0 - param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.0 - param set MC_YAW_P 1.0 - param set MC_YAWRATE_P 0.15 - param set MC_YAWRATE_I 0.0 + param set MC_ROLL_P 6.0 + param set MC_ROLLRATE_P 0.14 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_D 0.002 + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.002 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.15 + param set MC_YAW_FF 0.8 + param set BAT_V_SCALING 0.00838095238 fi From fe28069effe77dcac143c0194b982028438068f3 Mon Sep 17 00:00:00 2001 From: Kynos Date: Wed, 28 May 2014 17:26:41 +0200 Subject: [PATCH 081/105] Increase UART1 & UART5 RX&Tx buffer sizes To fix MAVLink message garbling problems. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index bc0c120679..f2c7d22bfe 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=256 -CONFIG_USART1_TXBUFSIZE=128 +CONFIG_USART1_RXBUFSIZE=300 +CONFIG_USART1_TXBUFSIZE=300 CONFIG_USART1_BAUD=57600 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # UART5 Configuration # -CONFIG_UART5_RXBUFSIZE=256 -CONFIG_UART5_TXBUFSIZE=128 +CONFIG_UART5_RXBUFSIZE=300 +CONFIG_UART5_TXBUFSIZE=300 CONFIG_UART5_BAUD=57600 CONFIG_UART5_BITS=8 CONFIG_UART5_PARITY=0 From bb6b442ca66a8349eb934a43b5886f44c2b7ab87 Mon Sep 17 00:00:00 2001 From: Liio Chen Date: Thu, 29 May 2014 17:42:05 +0800 Subject: [PATCH 082/105] Magnetometer data is not update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Magnetometer is not updated during a read operation, because the function "lsm303d_mag::measure" is not called. ”!!!JUST A GUESS!!!“ --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4ca8b5e42d..8bf76dcc33 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -880,7 +880,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - measure(); + _mag->measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) From 17d8e2a1663b36504a432b120c3993e4912842b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 12:24:50 +0200 Subject: [PATCH 083/105] PCA8574 driver: Cleanup, ready for final testing and production --- src/drivers/drv_io_expander.h | 18 +- src/drivers/pca8574/pca8574.cpp | 343 +++++++++++++------------------- 2 files changed, 149 insertions(+), 212 deletions(-) diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h index 2705d6f9e1..106354377f 100644 --- a/src/drivers/drv_io_expander.h +++ b/src/drivers/drv_io_expander.h @@ -49,17 +49,23 @@ #define _IOXIOCBASE (0x2800) #define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) -/** enable the device */ -#define IOX_ENABLE _IOXIOC(1) +/** set a bitmask (non-blocking) */ +#define IOX_SET_MASK _IOXIOC(1) -/** set constant values */ -#define IOX_SET_VALUE _IOXIOC(2) +/** get a bitmask (blocking) */ +#define IOX_GET_MASK _IOXIOC(2) -/** set constant values */ +/** set device mode (non-blocking) */ #define IOX_SET_MODE _IOXIOC(3) +/** set constant values (non-blocking) */ +#define IOX_SET_VALUE _IOXIOC(4) + +/* ... to IOX_SET_VALUE + 8 */ + /* enum passed to RGBLED_SET_MODE ioctl()*/ enum IOX_MODE { IOX_MODE_OFF, - IOX_MODE_ON + IOX_MODE_ON, + IOX_MODE_TEST_OUT }; diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index ce882b6367..cd1ffc82c6 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Julian Oes - * Anton Babushkin + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,8 +34,11 @@ /** * @file pca8574.cpp * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * Driver for an 8 I/O controller (PC8574) connected via I2C. * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -68,16 +69,7 @@ #define PCA8574_OFFTIME 120 #define PCA8574_DEVICE_PATH "/dev/pca8574" -#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ -#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ -#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ -#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ -#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ - -#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ -#define SETTING_ENABLE 0x02 /**< on */ - +#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) class PCA8574 : public device::I2C { @@ -94,20 +86,23 @@ public: private: work_s _work; - uint8_t _values[8]; - float _brightness; + uint8_t _values_out; + uint8_t _values_in; enum IOX_MODE _mode; bool _running; int _led_interval; bool _should_run; + bool _update_out; int _counter; static void led_trampoline(void *arg); void led(); - int send_led_enable(bool enable); + int send_led_enable(uint8_t arg); int send_led_values(); + + int get(uint8_t &vals); }; /* for now, we only support one PCA8574 */ @@ -122,12 +117,13 @@ extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _values_out(0), + _values_in(0), _mode(IOX_MODE_OFF), - _values({}), - _brightness(1.0f), _running(false), _led_interval(0), _should_run(false), + _update_out(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -147,10 +143,10 @@ PCA8574::init() return ret; } - /* switch off LED on start */ - send_led_enable(false); + // switch off LED on start (active low on Pixhawk) + send_led_enable(0xFF); - /* kick it in */ + // kick it in _should_run = true; _led_interval = 80; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); @@ -179,24 +175,69 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { - case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): - { - /* set the specified color */ - unsigned prev = _values[cmd - IOX_SET_VALUE]; - _values[cmd - IOX_SET_VALUE] = arg; - if (_values[cmd - IOX_SET_VALUE] != prev) { - // XXX will be done with a change flag - send_led_values(); - } - return OK; - } + case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { + // set the specified on / off state + uint8_t position = (1 << (cmd - IOX_SET_VALUE)); + uint8_t prev = _values_out; - case IOX_ENABLE: + if (arg) { + _values_out |= position; + + } else { + _values_out &= ~(position); + } + + if (_values_out != prev) { + send_led_values(); + } + + return OK; + } + + case IOX_SET_MASK: send_led_enable(arg); return OK; + case IOX_GET_MASK: { + uint8_t val; + ret = get(val); + + if (ret == OK) { + return val; + + } else { + return -1; + } + } + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + _values_out = 0xFF; + break; + + case IOX_MODE_ON: + _values_out = 0; + break; + + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + send_led_values(); + } + + return OK; + default: - /* see if the parent class can make any use of it */ + // see if the parent class can make any use of it ret = CDev::ioctl(filp, cmd, arg); break; } @@ -224,204 +265,93 @@ PCA8574::led() return; } - // switch (_mode) { - // case PCA8574_MODE_BLINK_SLOW: - // case PCA8574_MODE_BLINK_NORMAL: - // case PCA8574_MODE_BLINK_FAST: - // if (_counter >= 2) - // _counter = 0; + if (_mode == IOX_MODE_TEST_OUT) { - // send_led_enable(_counter == 0); + // we count only seven states + _counter &= 0xF; + _counter++; - // break; + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values_out |= (1 << i); - // case PCA8574_MODE_BREATHE: + } else { + _values_out &= ~(1 << i); + } + } - // if (_counter >= 62) - // _counter = 0; + _update_out = true; + } - // int n; + if (_update_out) { + uint8_t msg = _values_out; - // if (_counter < 32) { - // n = _counter; + int ret = transfer(&msg, sizeof(msg), nullptr, 0); - // } else { - // n = 62 - _counter; - // } - - // _brightness = n * n / (31.0f * 31.0f); - // send_led_rgb(); - // break; - - // case PCA8574_MODE_PATTERN: - - // /* don't run out of the pattern array and stop if the next frame is 0 */ - // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) - // _counter = 0; - - // set_color(_pattern.color[_counter]); - // send_led_rgb(); - // _led_interval = _pattern.duration[_counter]; - // break; - - // default: - // break; - // } - - - // we count only seven states - _counter &= 0xF; - _counter++; - - for (int i = 0; i < 8; i++) { - if (i < _counter) { - _values[i] = 1; - } else { - _values[i] = 0; + if (!ret) { + _update_out = false; } } - send_led_values(); + // check if any activity remains, else stp + if (!_update_out) { + _running = false; + return; + } - /* re-queue ourselves to run again later */ + // re-queue ourselves to run again later work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } -// /** -// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) -// */ -// void -// PCA8574::set_mode(pca8574_mode_t mode) -// { -// if (mode != _mode) { -// _mode = mode; - -// switch (mode) { -// // case PCA8574_MODE_OFF: -// // _should_run = false; -// // send_led_enable(false); -// // break; - -// // case PCA8574_MODE_ON: -// // _brightness = 1.0f; -// // send_led_rgb(); -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_BLINK_SLOW: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 2000; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_NORMAL: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 500; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_FAST: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 100; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BREATHE: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 25; -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_PATTERN: -// // _should_run = true; -// // _counter = 0; -// // _brightness = 1.0f; -// // send_led_enable(true); -// // break; - -// default: -// warnx("mode unknown"); -// break; -// } - -// /* if it should run now, start the workq */ -// if (_should_run && !_running) { -// _running = true; -// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); -// } - -// } -// } - /** * Sent ENABLE flag to LED driver */ int -PCA8574::send_led_enable(bool enable) +PCA8574::send_led_enable(uint8_t arg) { - uint8_t msg; - if (enable) { - /* active low */ - msg = 0x00; - } else { - /* active low, so off */ - msg = 0xFF; - } - - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + int ret = transfer(&arg, sizeof(arg), nullptr, 0); return ret; } /** - * Send RGB PWM settings to LED driver according to current color and brightness + * Send 8 outputs */ int PCA8574::send_led_values() { - uint8_t msg = 0; + _update_out = true; + _should_run = true; - for (int i = 0; i < 8; i++) { - if (_values[i]) { - msg |= (1 << i); - } + // if not active, kick it + if (!_running) { + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + return 0; } -// int -// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) -// { -// uint8_t result[2]; -// int ret; +int +PCA8574::get(uint8_t &vals) +{ + uint8_t result; + int ret; -// ret = transfer(nullptr, 0, &result[0], 2); + ret = transfer(nullptr, 0, &result, 1); -// if (ret == OK) { -// on = result[0] & SETTING_ENABLE; -// powersave = !(result[0] & SETTING_NOT_POWERSAVE); -// /* XXX check, looks wrong */ -// r = (result[0] & 0x0f) << 4; -// g = (result[1] & 0xf0); -// b = (result[1] & 0x0f) << 4; -// } + if (ret == OK) { + _values_in = result; + vals = result; + } -// return ret; -// } + return ret; +} void pca8574_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -431,11 +361,11 @@ int pca8574_main(int argc, char *argv[]) { int i2cdevice = -1; - int pca8574adr = ADDR; /* 7bit */ + int pca8574adr = ADDR; // 7bit int ch; - /* jump over start/off/etc and look at options first */ + // jump over start/off/etc and look at options first while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': @@ -452,10 +382,10 @@ pca8574_main(int argc, char *argv[]) } } - if (optind >= argc) { - pca8574_usage(); - exit(1); - } + if (optind >= argc) { + pca8574_usage(); + exit(1); + } const char *verb = argv[optind]; @@ -463,8 +393,9 @@ pca8574_main(int argc, char *argv[]) int ret; if (!strcmp(verb, "start")) { - if (g_pca8574 != nullptr) + if (g_pca8574 != nullptr) { errx(1, "already started"); + } if (i2cdevice == -1) { // try the external bus first @@ -481,6 +412,7 @@ pca8574_main(int argc, char *argv[]) if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { errx(1, "init failed"); } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -488,8 +420,9 @@ pca8574_main(int argc, char *argv[]) if (g_pca8574 == nullptr) { g_pca8574 = new PCA8574(i2cdevice, pca8574adr); - if (g_pca8574 == nullptr) + if (g_pca8574 == nullptr) { errx(1, "new failed"); + } if (OK != g_pca8574->init()) { delete g_pca8574; @@ -501,7 +434,7 @@ pca8574_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + // need the driver past this point if (g_pca8574 == nullptr) { warnx("not started"); pca8574_usage(); @@ -515,8 +448,7 @@ pca8574_main(int argc, char *argv[]) errx(1, "Unable to open " PCA8574_DEVICE_PATH); } - ret = ioctl(fd, IOX_SET_VALUE, 255); - // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); close(fd); exit(ret); @@ -547,7 +479,7 @@ pca8574_main(int argc, char *argv[]) if (!strcmp(verb, "val")) { if (argc < 4) { - errx(1, "Usage: pca8574 val "); + errx(1, "Usage: pca8574 val <0 or 1>"); } fd = open(PCA8574_DEVICE_PATH, 0); @@ -558,8 +490,7 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE+channel), val); - ret = ioctl(fd, IOX_ENABLE, 1); + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); close(fd); exit(ret); } From 0c2e70d30f3e1d927a22fdd066a4a14ecc698de1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:29 +0200 Subject: [PATCH 084/105] Enable the driver as default for boards having it --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c413e0878..87ec4293ea 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -93,14 +93,18 @@ then # if rgbled start then - echo "[init] Using external RGB Led" + echo "[init] RGB Led" else if blinkm start then - echo "[init] Using blinkm" + echo "[init] BlinkM" blinkm systemstate fi fi + + if pca8574 start + then + fi # # Set default values From eb02e74d30a215ff415983913bf569a23e916a2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:47 +0200 Subject: [PATCH 085/105] PCA cleanup: Full interfaces ready for AP use --- src/drivers/pca8574/pca8574.cpp | 108 ++++++++++++++++++++++++-------- 1 file changed, 81 insertions(+), 27 deletions(-) diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index cd1ffc82c6..904ce18e8a 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -82,12 +82,15 @@ public: virtual int probe(); virtual int info(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } private: work_s _work; uint8_t _values_out; uint8_t _values_in; + uint8_t _blinking; + uint8_t _blink_phase; enum IOX_MODE _mode; bool _running; @@ -119,9 +122,11 @@ PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), _values_out(0), _values_in(0), + _blinking(0), + _blink_phase(0), _mode(IOX_MODE_OFF), _running(false), - _led_interval(0), + _led_interval(80), _should_run(false), _update_out(false), _counter(0) @@ -143,22 +148,14 @@ PCA8574::init() return ret; } - // switch off LED on start (active low on Pixhawk) - send_led_enable(0xFF); - - // kick it in - _should_run = true; - _led_interval = 80; - work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); - return OK; } int PCA8574::probe() { - - return send_led_enable(false); + uint8_t val; + return get(val); } int @@ -188,6 +185,9 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) } if (_values_out != prev) { + if (_values_out) { + _mode = IOX_MODE_ON; + } send_led_values(); } @@ -260,11 +260,6 @@ PCA8574::led_trampoline(void *arg) void PCA8574::led() { - if (!_should_run) { - _running = false; - return; - } - if (_mode == IOX_MODE_TEST_OUT) { // we count only seven states @@ -281,10 +276,37 @@ PCA8574::led() } _update_out = true; + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _update_out = true; + _should_run = false; + } else { + + // Any of the normal modes + if (_blinking > 0) { + /* we need to be running to blink */ + _should_run = true; + } else { + _should_run = false; + } } if (_update_out) { - uint8_t msg = _values_out; + uint8_t msg; + + if (_blinking) { + msg = (_values_out & _blinking & _blink_phase); + + // wipe out all positions that are marked as blinking + msg &= ~(_blinking); + + // fill blink positions + msg |= ((_blink_phase) ? _blinking : 0); + + _blink_phase = !_blink_phase; + } else { + msg = _values_out; + } int ret = transfer(&msg, sizeof(msg), nullptr, 0); @@ -294,12 +316,13 @@ PCA8574::led() } // check if any activity remains, else stp - if (!_update_out) { + if (!_should_run) { _running = false; return; } // re-queue ourselves to run again later + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } @@ -322,10 +345,10 @@ int PCA8574::send_led_values() { _update_out = true; - _should_run = true; // if not active, kick it if (!_running) { + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } @@ -436,8 +459,7 @@ pca8574_main(int argc, char *argv[]) // need the driver past this point if (g_pca8574 == nullptr) { - warnx("not started"); - pca8574_usage(); + warnx("not started, run pca8574 start"); exit(1); } @@ -459,10 +481,10 @@ pca8574_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + if (!strcmp(verb, "off")) { fd = open(PCA8574_DEVICE_PATH, 0); - if (fd == -1) { + if (fd < 0) { errx(1, "Unable to open " PCA8574_DEVICE_PATH); } @@ -472,9 +494,36 @@ pca8574_main(int argc, char *argv[]) } if (!strcmp(verb, "stop")) { - delete g_pca8574; - g_pca8574 = nullptr; - exit(0); + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca8574->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca8574->is_running()) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } else { + warnx("stop failed."); + exit(1); + } } if (!strcmp(verb, "val")) { @@ -490,7 +539,12 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + + if (channel < 8) { + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { + ret = -1; + } close(fd); exit(ret); } From 46301f753d212b55e151a394bc9d4b3b787f35ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 18:28:37 +0200 Subject: [PATCH 086/105] Minor fixes to MAVLink --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef6..28cdf65a3c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), From ead91f325989cda312424ec3b2cc7fd11913f5c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 29 May 2014 22:42:33 +0200 Subject: [PATCH 087/105] position_estimator_inav: GPS delay compensation --- .../position_estimator_inav_main.c | 50 ++++++++++++++++--- .../position_estimator_inav_params.c | 3 ++ .../position_estimator_inav_params.h | 2 + 3 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d7503e42da..d6a44304d2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,8 @@ #include "inertial_filter.h" #define MIN_VALID_W 0.00001f +#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz +#define EST_BUF_SIZE 500000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -83,7 +85,6 @@ static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); +static inline int min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +static inline int max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + /** * Print the correct usage. */ @@ -199,6 +210,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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]; + memset(est_buf, 0, sizeof(est_buf)); + int est_buf_ptr = 0; + float eph = 1.0; float epv = 1.0; @@ -657,16 +672,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) y_est[1] = gps.vel_e_m_s; } + /* calculate index of estimated values in buffer */ + int est_i = est_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] - x_est[0]; - corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; + 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 - x_est[1]; - corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; - corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; + 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; @@ -910,8 +931,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t > pub_last + pub_interval) { + if (t > pub_last + PUB_INTERVAL) { pub_last = t; + + /* push current estimate to FIFO buffer */ + est_buf[est_buf_ptr][0][0] = x_est[0]; + est_buf[est_buf_ptr][0][1] = x_est[1]; + est_buf[est_buf_ptr][1][0] = y_est[0]; + est_buf[est_buf_ptr][1][1] = y_est[1]; + est_buf[est_buf_ptr][2][0] = z_est[0]; + est_buf[est_buf_ptr][2][1] = z_est[1]; + est_buf_ptr++; + if (est_buf_ptr >= EST_BUF_SIZE) { + est_buf_ptr = 0; + } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8aa08b6f2c..8509d15cba 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; } @@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->delay_gps, &(p->delay_gps)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08ec996a13..df1cfaa710 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + float delay_gps; }; struct position_estimator_inav_param_handles { @@ -74,6 +75,7 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t delay_gps; }; /** From fdd5d7b8b8835dd3e2655cd104f7440c9f56ba27 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 11:03:06 +0200 Subject: [PATCH 088/105] position_estimator_inav: add buffer for rotation matrix to do accel bias correction properly --- .../position_estimator_inav_main.c | 61 ++++++++++++++----- 1 file changed, 45 insertions(+), 16 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d6a44304d2..aa533c777f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -72,7 +72,7 @@ #define MIN_VALID_W 0.00001f #define PUB_INTERVAL 10000 // limit publish rate to 100 Hz -#define EST_BUF_SIZE 500000 / PUB_INTERVAL // buffer size is 0.5s +#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -146,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); @@ -210,9 +210,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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]; + 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)); - int est_buf_ptr = 0; + memset(R_buf, 0, sizeof(R_buf)); + memset(R_gps, 0, sizeof(R_gps)); + int buf_ptr = 0; float eph = 1.0; float epv = 1.0; @@ -673,7 +677,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* calculate index of estimated values in buffer */ - int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); + 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; } @@ -695,6 +699,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + /* save rotation matrix at this moment */ + memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); + eph = fminf(eph, gps.eph_m); epv = fminf(epv, gps.epv_m); @@ -784,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_baro += offs_corr; } - /* accelerometer bias correction */ + /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { @@ -798,6 +805,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_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 += R_gps[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; @@ -934,16 +959,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + PUB_INTERVAL) { pub_last = t; - /* push current estimate to FIFO buffer */ - est_buf[est_buf_ptr][0][0] = x_est[0]; - est_buf[est_buf_ptr][0][1] = x_est[1]; - est_buf[est_buf_ptr][1][0] = y_est[0]; - est_buf[est_buf_ptr][1][1] = y_est[1]; - est_buf[est_buf_ptr][2][0] = z_est[0]; - est_buf[est_buf_ptr][2][1] = z_est[1]; - est_buf_ptr++; - if (est_buf_ptr >= EST_BUF_SIZE) { - est_buf_ptr = 0; + /* 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 */ From 718206bd6d4103d8d2b1ad6a111770f65622029a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 14:37:23 +0200 Subject: [PATCH 089/105] ubx driver: fix unit of speed and position accuracy estimate --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beecf..7502809bd7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -447,8 +447,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; From 0bdde089243bbf7e3651f9535f5c847f894e0ac7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:18:03 +0200 Subject: [PATCH 090/105] position_estimator_inav: default GPS delay changed to 0.2s --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8509d15cba..d88419c258 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,7 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); -PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 47c9d326209034dc373ab54647d29df4e63b5285 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:23:26 +0200 Subject: [PATCH 091/105] ubx: disable all debug messages --- src/drivers/gps/ubx.cpp | 170 ++++++++++++++++++++-------------------- 1 file changed, 85 insertions(+), 85 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 7502809bd7..cbfb61d00a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,19 +219,19 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; - } - - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); - return 1; - } +// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: NAV SVINFO"); +// return 1; +// } +// +// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: MON HW"); +// return 1; +// } _configured = true; return 0; @@ -486,61 +486,61 @@ UBX::handle_message() break; } - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - uint8_t satellites_used = 0; - int i; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - - /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; - break; - } +// case UBX_MESSAGE_NAV_SVINFO: { +// //printf("GOT NAV_SVINFO\n"); +// const int length_part1 = 8; +// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; +// const int length_part2 = 12; +// gps_bin_nav_svinfo_part2_packet_t *packet_part2; +// +// uint8_t satellites_used = 0; +// int i; +// +// //printf("Number of Channels: %d\n", packet_part1->numCh); +// for (i = 0; i < packet_part1->numCh; i++) { +// /* set pointer to sattelite_i information */ +// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); +// +// /* write satellite information to global storage */ +// uint8_t sv_used = packet_part2->flags & 0x01; +// +// if (sv_used) { +// /* count SVs used for NAV */ +// satellites_used++; +// } +// +// /* record info for all channels, whether or not the SV is used for NAV */ +// _gps_position->satellite_used[i] = sv_used; +// _gps_position->satellite_snr[i] = packet_part2->cno; +// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); +// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); +// _gps_position->satellite_prn[i] = packet_part2->svid; +// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); +// } +// +// for (i = packet_part1->numCh; i < 20; i++) { +// /* unused channels have to be set to zero for e.g. MAVLink */ +// _gps_position->satellite_prn[i] = 0; +// _gps_position->satellite_used[i] = 0; +// _gps_position->satellite_snr[i] = 0; +// _gps_position->satellite_elevation[i] = 0; +// _gps_position->satellite_azimuth[i] = 0; +// } +// +// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones +// +// if (packet_part1->numCh > 0) { +// _gps_position->satellite_info_available = true; +// +// } else { +// _gps_position->satellite_info_available = false; +// } +// +// _gps_position->timestamp_satellites = hrt_absolute_time(); +// +// ret = 1; +// break; +// } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +573,23 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { - switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; - - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; - - ret = 1; - break; - } - - default: - break; - } - } +// case UBX_CLASS_MON: { +// switch (_message_id) { +// case UBX_MESSAGE_MON_HW: { +// +// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; +// +// _gps_position->noise_per_ms = p->noisePerMS; +// _gps_position->jamming_indicator = p->jamInd; +// +// ret = 1; +// break; +// } +// +// default: +// break; +// } +// } default: break; From efb44969d584e1b2613d96a795fc94a3ef728d9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:57:48 +0200 Subject: [PATCH 092/105] ubx: send update only if got POSLLH & VELNED & TIMEUTC --- src/drivers/gps/ubx.cpp | 184 +++++++++++++++++++++------------------- src/drivers/gps/ubx.h | 3 + 2 files changed, 100 insertions(+), 87 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index cbfb61d00a..c143eeb0c2 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _gps_position(gps_position), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), + _got_timeutc(false), _disable_cmd_last(0) { decode_init(); @@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate) return 1; } -// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: NAV SVINFO"); -// return 1; -// } -// -// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: MON HW"); -// return 1; -// } + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } + + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } _configured = true; return 0; @@ -275,9 +278,10 @@ UBX::receive(unsigned timeout) bool handled = false; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; + _got_timeutc = false; return 1; } else { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -482,65 +490,66 @@ UBX::handle_message() _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); + _got_timeutc = true; ret = 1; break; } -// case UBX_MESSAGE_NAV_SVINFO: { -// //printf("GOT NAV_SVINFO\n"); -// const int length_part1 = 8; -// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; -// const int length_part2 = 12; -// gps_bin_nav_svinfo_part2_packet_t *packet_part2; -// -// uint8_t satellites_used = 0; -// int i; -// -// //printf("Number of Channels: %d\n", packet_part1->numCh); -// for (i = 0; i < packet_part1->numCh; i++) { -// /* set pointer to sattelite_i information */ -// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); -// -// /* write satellite information to global storage */ -// uint8_t sv_used = packet_part2->flags & 0x01; -// -// if (sv_used) { -// /* count SVs used for NAV */ -// satellites_used++; -// } -// -// /* record info for all channels, whether or not the SV is used for NAV */ -// _gps_position->satellite_used[i] = sv_used; -// _gps_position->satellite_snr[i] = packet_part2->cno; -// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); -// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); -// _gps_position->satellite_prn[i] = packet_part2->svid; -// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); -// } -// -// for (i = packet_part1->numCh; i < 20; i++) { -// /* unused channels have to be set to zero for e.g. MAVLink */ -// _gps_position->satellite_prn[i] = 0; -// _gps_position->satellite_used[i] = 0; -// _gps_position->satellite_snr[i] = 0; -// _gps_position->satellite_elevation[i] = 0; -// _gps_position->satellite_azimuth[i] = 0; -// } -// -// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones -// -// if (packet_part1->numCh > 0) { -// _gps_position->satellite_info_available = true; -// -// } else { -// _gps_position->satellite_info_available = false; -// } -// -// _gps_position->timestamp_satellites = hrt_absolute_time(); -// -// ret = 1; -// break; -// } + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + + uint8_t satellites_used = 0; + int i; + + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); + } + + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; + } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } @@ -573,23 +583,23 @@ UBX::handle_message() break; } -// case UBX_CLASS_MON: { -// switch (_message_id) { -// case UBX_MESSAGE_MON_HW: { -// -// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; -// -// _gps_position->noise_per_ms = p->noisePerMS; -// _gps_position->jamming_indicator = p->jamInd; -// -// ret = 1; -// break; -// } -// -// default: -// break; -// } -// } + case UBX_CLASS_MON: { + switch (_message_id) { + case UBX_MESSAGE_MON_HW: { + + struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + + _gps_position->noise_per_ms = p->noisePerMS; + _gps_position->jamming_indicator = p->jamInd; + + ret = 1; + break; + } + + default: + break; + } + } default: break; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b5..43d6888936 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; From 138edca5445a66df3647c03529ad2a30a51fe083 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 31 May 2014 13:15:10 +0200 Subject: [PATCH 093/105] attempt to fix mavlink hardware flow control disable logic --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28cdf65a3c..540b886579 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -105,7 +105,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { @@ -199,6 +199,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { warnx("TX FAIL"); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } From 6b8248ee29f38ae0f2ff10ebc23b1816e1fc6829 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 31 May 2014 16:19:38 +0200 Subject: [PATCH 094/105] position_estimator_inav: more safe EPH/EPV estimation, minor changes --- .../position_estimator_inav_main.c | 47 ++++++++++++------- 1 file changed, 30 insertions(+), 17 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index aa533c777f..f908d7a3be 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -79,7 +79,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss @@ -218,8 +218,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; - float eph = 1.0; - float epv = 1.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 x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); @@ -276,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; - static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation - static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation - 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) @@ -555,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) w_flow *= 0.05f; } - flow_valid = true; + /* under ideal conditions, on 1m distance assume EPH = 10cm */ + eph_flow = 0.1 / w_flow; - eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + flow_valid = true; } else { w_flow = 0.0f; @@ -616,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -702,9 +705,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - eph = fminf(eph, gps.eph_m); - epv = fminf(epv, gps.epv_m); - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -745,8 +745,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ - eph *= 1.0 + dt; - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + if (eph < max_eph_epv) { + eph *= 1.0 + dt; + } + if (epv < max_eph_epv) { + epv += 0.005 * 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; @@ -759,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -853,7 +857,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); - inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + + if (use_gps_z) { + epv = fminf(epv, gps.epv_m); + + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_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); @@ -878,11 +887,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* 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_m); + 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); From 9ee42e8e5986dcdb2e5bc9ff5110ad7bed462f98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 10:59:11 +0200 Subject: [PATCH 095/105] rcS: fix whitespace --- ROMFS/px4fmu_common/init.d/rcS | 82 +++++++++++++++++----------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 87ec4293ea..6d06f897a3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -65,12 +65,12 @@ then # Start CDC/ACM serial driver # sercon - + # # Start the ORB (first app to start) # uorb start - + # # Load parameters # @@ -79,7 +79,7 @@ then then set PARAM_FILE /fs/mtd_params fi - + param select $PARAM_FILE if param load then @@ -87,7 +87,7 @@ then else echo "[init] ERROR: Params loading failed: $PARAM_FILE" fi - + # # Start system state indicator # @@ -105,7 +105,7 @@ then if pca8574 start then fi - + # # Set default values # @@ -126,7 +126,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no - + # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # @@ -136,7 +136,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set USE_IO flag # @@ -146,7 +146,7 @@ then else set USE_IO no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -176,9 +176,9 @@ then param set SYS_AUTOCONFIG 0 param save fi - + set IO_PRESENT no - + if [ $USE_IO == yes ] then # @@ -190,19 +190,19 @@ then else set IO_FILE /etc/extras/px4io-v1_default.bin fi - + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE - + set IO_PRESENT yes else echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE - + tone_alarm MLL32CP8MB - + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -211,7 +211,7 @@ then echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE - + set IO_PRESENT yes else echo "[init] ERROR: PX4IO update failed" @@ -224,14 +224,14 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $IO_PRESENT == no ] then echo "[init] ERROR: PX4IO not found" tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # @@ -250,7 +250,7 @@ then # Need IO for output but it not present, disable output set OUTPUT_MODE none echo "[init] ERROR: PX4IO not found, disabling output" - + # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 then @@ -274,17 +274,17 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & - + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - + # # Start primary output # set TTYS1_BUSY no - + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then @@ -300,7 +300,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -311,7 +311,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -324,7 +324,7 @@ then fi fi fi - + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -337,7 +337,7 @@ then then set MKBLCTRL_ARG "-mkmode +" fi - + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" @@ -345,9 +345,9 @@ then echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi - + fi - + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -359,7 +359,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Start IO or FMU for RC PPM input if needed # @@ -386,7 +386,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -401,7 +401,7 @@ then fi fi fi - + # # MAVLink # @@ -422,7 +422,7 @@ then fi mavlink start $MAVLINK_FLAGS - + # # Sensors, Logging, GPS # @@ -433,7 +433,7 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - + if [ $GPS == yes ] then echo "[init] Start GPS" @@ -443,7 +443,7 @@ then gps start -f else gps start - fi + fi fi # @@ -460,24 +460,24 @@ then if [ $VEHICLE_TYPE == fw ] then echo "[init] Vehicle type: FIXED WING" - + if [ $MIXER == none ] then # Set default mixer for fixed wing if not defined set MIXER FMU_AERT fi - + if [ $MAV_TYPE == none ] then # Use MAV_TYPE = 1 (fixed wing) if not defined set MAV_TYPE 1 fi - + param set MAV_TYPE $MAV_TYPE - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard fixedwing apps if [ $LOAD_DEFAULT_APPS == yes ] then @@ -525,7 +525,7 @@ then set MAV_TYPE 14 fi fi - + # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then @@ -533,10 +533,10 @@ then else param set MAV_TYPE $MAV_TYPE fi - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard multicopter apps if [ $LOAD_DEFAULT_APPS == yes ] then From 0c35b7a8ee5305b8cc4c2dda69222f137f3f3593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 11:50:14 +0200 Subject: [PATCH 096/105] Fixed EKF initial param values --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4ed..97644d1b9a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -37,7 +37,7 @@ then param set MPC_LAND_SPEED 1.0 param set PE_VELNE_NOISE 0.5 - param set PE_VELNE_NOISE 0.7 + param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 From 480c41d83591354480a8fd81cfbc314fccaa6a20 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 13:18:56 +0200 Subject: [PATCH 097/105] do not read out home position in gcs (home position is still displayed) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a166..65922b2a5d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { From 794b853a3b94b5520b734933f18823885259427d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 14:04:16 +0200 Subject: [PATCH 098/105] mavlink: for takeoff mission items don't set the time inside field. Correctly set pitch min when converting from mission item to mavlink mission item --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 540b886579..304a02c291 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,6 +896,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item default: mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; break; } @@ -904,7 +905,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -923,11 +923,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; + mavlink_mission_item->param1 = mission_item->pitch_min; break; default: mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; break; } @@ -938,7 +939,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; From ad811304c89d00d6432ec871916b7a82c8b3cbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:36:26 +0200 Subject: [PATCH 099/105] actuator_armed: Fixed comments and doxygen, no C-level changes --- src/modules/uORB/topics/actuator_armed.h | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 6e944ffee3..a98d3fc3a4 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,15 +44,25 @@ #include #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + /** global 'actuator output is live' control. */ struct actuator_armed_s { - uint64_t timestamp; - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + uint64_t timestamp; /**< Microseconds since system boot */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; +/** + * @} + */ + +/* register this as object request broker structure */ ORB_DECLARE(actuator_armed); #endif \ No newline at end of file From 325d5026bb6d36ab3557ab999f8db772f77ac7a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 13:37:31 +0200 Subject: [PATCH 100/105] Hotfix: Fix scaling for battery current --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f560..fd41b723ab 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -274,7 +274,7 @@ protected: status->onboard_control_sensors_health, status->load * 1000.0f, status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, + status->battery_current * 100.0f, status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, From 5624c1406aa78aa4bf4b5c0e20dca637c26478d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:43:15 +0200 Subject: [PATCH 101/105] Hotfix: Better microSD reporting --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 304a02c291..264cfd875a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1054,6 +1054,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } @@ -1440,6 +1441,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } From 33067373614e50e3be068d30f3ad3b718d16df5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 6 Jun 2014 00:42:02 +0200 Subject: [PATCH 102/105] mavlink: send MISSION_REQUEST after short timeout when receiving mission, remove all "target id mismatch" warnings --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++-------------------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 15 insertions(+), 37 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 264cfd875a..e300be0747 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -790,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_param_request_list_t req; + mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -954,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_compid = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; } @@ -1070,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.seq = seq; mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); + _wpm->timestamp_last_send_request = hrt_absolute_time(); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -1112,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; + + } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* try to get WP again after short timeout */ + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1174,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1211,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1304,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1368,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1473,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1513,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 25c0da8206..40edc4b851 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -93,6 +93,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_last_send_request; uint32_t timeout; int current_dataman_id; }; From c4e2232078f2f28cbf97b8280f40fd988a5c2a75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Jun 2014 08:13:05 +0200 Subject: [PATCH 103/105] Remove unused loiter radius parameter. Fixes #1042 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ------ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 11 ----------- 2 files changed, 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5b877cd77d..116d3cc63d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -232,8 +232,6 @@ private: float throttle_land_max; - float loiter_hold_radius; - float heightrate_p; float speedrate_p; @@ -277,8 +275,6 @@ private: param_t throttle_land_max; - param_t loiter_hold_radius; - param_t heightrate_p; param_t speedrate_p; @@ -441,7 +437,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); - _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -513,7 +508,6 @@ FixedwingPositionControl::parameters_update() /* L1 control parameters */ param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); - param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f258f77dab..52128e1b7a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -71,17 +71,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); -/** - * Default Loiter Radius - * - * This radius is used when no other loiter radius is set. - * - * @min 10.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - /** * Cruise throttle * From 85b2dfa0c662298d49f584e6bd774954b04cec35 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Jun 2014 12:30:27 +0200 Subject: [PATCH 104/105] fix initialization of perfcounters in fw att controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0a909d02f5..a0a18bc2e4 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); } ECL_PitchController::~ECL_PitchController() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 82903ef5aa..d2a231694d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); } ECL_RollController::~ECL_RollController() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e53ffc6448..79184e2cdd 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated_min_speed(1.0f) + _coordinated_min_speed(1.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); } ECL_YawController::~ECL_YawController() From 128389d3b14a43d48712b2bb5e32cac85b5dafcc Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Mon, 9 Jun 2014 16:01:44 -0700 Subject: [PATCH 105/105] Converted style to work with wiki. Cleaned up bad fields. --- .../fw_att_control/fw_att_control_params.c | 367 +++++++++++++----- 1 file changed, 270 insertions(+), 97 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index aa637e2074..7cae846785 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -50,149 +50,322 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Controller parameters, accessible via MAVLink * */ -// @DisplayName Attitude Time Constant -// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. -// @Range 0.4 to 1.0 seconds, in tens of seconds + +/** + * Attitude Time Constant + * + * This defines the latency between a step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit seconds + * @min 0.4 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); -// @DisplayName Pitch rate proportional gain. -// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error. -// @Range 10 to 200, 1 increments +/** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); -// @DisplayName Pitch rate integrator gain. -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0 to 50.0 +/** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); -// @DisplayName Maximum positive / up pitch rate. -// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds, in 1 increments +/** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); -// @DisplayName Maximum negative / down pitch rate. -// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds, in 1 increments +/** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); -// @DisplayName Pitch rate integrator limit -// @Description The portion of the integrator part in the control surface deflection is limited to this value -// @Range 0.0 to 1 -// @Increment 0.1 +/** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); -// @DisplayName Roll to Pitch feedforward gain. -// @Description This compensates during turns and ensures the nose stays level. -// @Range 0.5 2.0 -// @Increment 0.05 -// @User User +/** + * Roll to Pitch feedforward gain. + * + * This compensates during turns and ensures the nose stays level. + * + * @min 0.0 + * @max 2.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) -// @DisplayName Roll rate proportional Gain. -// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. -// @Range 10.0 200.0 -// @Increment 10.0 -// @User User +/** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); -// @DisplayName Roll rate integrator Gain -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0.0 100.0 -// @Increment 5.0 -// @User User +/** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 100.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); -// @DisplayName Roll Integrator Anti-Windup -// @Description The portion of the integrator part in the control surface deflection is limited to this value. -// @Range 0.0 to 1.0 -// @Increment 0.1 +/** + * Roll Integrator Anti-Windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); -// @DisplayName Maximum Roll Rate -// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_RMAX, 0); +/** + * Maximum Roll Rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); -// @DisplayName Yaw rate proportional gain. -// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error. -// @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_YR_P, 0.05); +/** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); -// @DisplayName Yaw rate integrator gain. -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0 to 50.0 +/** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); -// @DisplayName Yaw rate integrator limit -// @Description The portion of the integrator part in the control surface deflection is limited to this value -// @Range 0.0 to 1 -// @Increment 0.1 +/** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); -// @DisplayName Maximum Yaw Rate -// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0); +/** + * Maximum Yaw Rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); -// @DisplayName Roll rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); -// @DisplayName Pitch rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); -// @DisplayName Yaw rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); -// @DisplayName Minimal speed for yaw coordination -// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 +/** + * Minimal speed for yaw coordination + * + * For airspeeds above this value, the yaw rate is calculated for a coordinated + * turn. Set to a very high value to disable. + * + * @unit m/s + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); -/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */ +/* Airspeed parameters: + * The following parameters about airspeed are used by the attitude and the + * position controller. + * */ -// @DisplayName Minimum Airspeed -// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively -// @Range 0.0 to 30 +/** + * Minimum Airspeed + * + * If the airspeed falls below this value, the TECS controller will try to + * increase airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); -// @DisplayName Trim Airspeed -// @Description The TECS controller tries to fly at this airspeed -// @Range 0.0 to 30 +/** + * Trim Airspeed + * + * The TECS controller tries to fly at this airspeed. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); -// @DisplayName Maximum Airspeed -// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively -// @Range 0.0 to 30 +/** + * Maximum Airspeed + * + * If the airspeed is above this value, the TECS controller will try to decrease + * airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); -// @DisplayName Roll Setpoint Offset -// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe -// @Range -90.0 to 90.0 +/** + * Roll Setpoint Offset + * + * An airframe specific offset of the roll setpoint in degrees, the value is + * added to the roll setpoint and should correspond to the typical cruise speed + * of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); -// @DisplayName Pitch Setpoint Offset -// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe -// @Range -90.0 to 90.0 +/** + * Pitch Setpoint Offset + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the typical cruise + * speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -// @DisplayName Max Manual Roll -// @Description Max roll for manual control in attitude stabilized mode -// @Range 0.0 to 90.0 +/** + * Max Manual Roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); -// @DisplayName Max Manual Pitch -// @Description Max pitch for manual control in attitude stabilized mode -// @Range 0.0 to 90.0 +/** + * Max Manual Pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);