From 1dc9785083e1ed7a7db27466fbe9f61f6bb277f7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 16:52:35 +0100 Subject: OFFBOARD switch added, WIP --- src/modules/commander/commander.cpp | 11 +++++++++ src/modules/sensors/sensor_params.c | 3 +-- src/modules/sensors/sensors.cpp | 29 ++++++++++------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 +--- src/modules/uORB/topics/vehicle_status.h | 8 +++++++ 5 files changed, 34 insertions(+), 21 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838c..e2b6ad1ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1410,6 +1410,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* offboard switch */ + if (!isfinite(sp_man->offboard_switch)) { + current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + + } else { + current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + } } transition_result_t diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef93..eb771382d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -390,8 +390,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); - -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e8c5edbc..f87f2f282 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,8 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; - -// int rc_map_offboard_ctrl_mode_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -296,8 +295,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; - -// param_t rc_map_offboard_ctrl_mode_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -515,8 +513,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_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -669,14 +666,14 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("Failed getting offboard sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan index"); -// } - param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); @@ -700,11 +697,10 @@ 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[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -1289,7 +1285,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; + manual_control.offboard_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1432,9 +1428,10 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* offboard switch input */ + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..ab0c7a411 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,15 +60,13 @@ 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 offboard_switch; /**< offboard switch (optional): offboard, onboard */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..d74b696bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_OFFBOARD, } main_state_t; typedef enum { @@ -109,6 +110,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + OFFBOARD_SWITCH_NONE = 0, + OFFBOARD_SWITCH_OFFBOARD, + OFFBOARD_SWITCH_ONBOARD +} offboard_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -184,6 +191,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + offboard_switch_pos_t offboard_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ -- cgit v1.2.3 From 37e28e7fa82a965967abba6650ba24a31aba10d3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 17:04:07 +0100 Subject: commander: OFFBOARD mode added, WIP --- src/modules/commander/commander.cpp | 80 ++++++++++++++------------ src/modules/commander/state_machine_helper.cpp | 6 ++ 2 files changed, 49 insertions(+), 37 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2b6ad1ca..dfcb1cc9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1429,56 +1429,62 @@ check_main_state_machine(struct vehicle_status_s *current_status) /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { - case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + /* offboard switch overrides main switch */ + res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + } else { + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - default: - break; + default: + break; + } } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..e5599e1c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -262,6 +262,12 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m ret = TRANSITION_CHANGED; } + break; + + case MAIN_STATE_OFFBOARD: + + ret = TRANSITION_CHANGED; + break; } -- cgit v1.2.3 From f069fe9f61d24b5a6cc66d60afd3f8b5e568824a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 19:25:14 +0100 Subject: OFFBOARD mode support in commander, mavlink, mc_att_control, WIP --- src/modules/commander/commander.cpp | 66 +++++++++------ src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 2 + src/modules/mc_att_control/mc_att_control_main.cpp | 99 ++++++++++++---------- src/modules/navigator/navigator_main.cpp | 16 +++- 5 files changed, 110 insertions(+), 74 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfcb1cc9f..7b69f2b76 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -411,42 +411,52 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (status->rc_signal_lost) { + /* allow mode switching by command only if no RC signal */ + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); + } + + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } } } + } else { + mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } + if (main_res == TRANSITION_CHANGED) ret = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..4843e72c3 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_OFFBOARD, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..70229558c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -231,6 +231,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; } *mavlink_custom_mode = custom_mode.data; 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 245ac024b..c4c6c0138 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -480,64 +480,73 @@ MulticopterAttitudeControl::task_main() float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + if (_control_mode.flag_control_offboard_enabled) { + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode + // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode + // but it's not very beautiful, need to think how to do it better - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + } else { + /* onboard control */ + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; _att_sp.R_valid = false; publish_att_sp = true; } - } - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; + } - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } - - } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; } if (_att_sp.R_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca5735509..7285e243e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1463,11 +1463,11 @@ Navigator::publish_control_mode() _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_flighttermination_enabled = false; switch (_vstatus.main_state) { case MAIN_STATE_MANUAL: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; @@ -1478,6 +1478,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_SEATBELT: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1488,6 +1489,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_EASY: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1498,6 +1500,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_AUTO: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1517,6 +1520,17 @@ Navigator::publish_control_mode() } break; + case MAIN_STATE_OFFBOARD: + _control_mode.flag_control_offboard_enabled = true; + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _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; + default: break; } -- cgit v1.2.3 From 6bb85a323ce7f152aead0c9133d86e5e900bd7ba Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Thu, 23 Jan 2014 11:01:22 +0000 Subject: added offboard data handling in mc_att_control_main.cpp --- src/modules/mc_att_control/mc_att_control_main.cpp | 67 +++++++++++++++++++++- 1 file changed, 64 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 c4c6c0138..129e5df8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -118,6 +119,7 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ + int _offboard_sub; /**< notification of offboard control updates */ int _arming_sub; /**< arming status of outputs */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -127,6 +129,7 @@ private: struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct offboard_control_setpoint_s _offboard; /**< offboard data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -173,6 +176,11 @@ private: * Check for changes in manual inputs. */ void vehicle_manual_poll(); + + /** + * Check for changes in offboard inputs. + */ + void vehicle_offboard_poll(); /** * Check for set triplet updates. @@ -339,6 +347,23 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } +void +MulticopterAttitudeControl::vehicle_offboard_poll() +{ + bool offboard_updated; + + /* get offboard inputs */ + orb_check(_offboard_sub, &offboard_updated); + + if (offboard_updated) { + + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); + } + +} + + + void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -363,6 +388,8 @@ MulticopterAttitudeControl::arming_status_poll() } } + + void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -476,15 +503,49 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); + vehicle_offboard_poll(); float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode // but it's not very beautiful, need to think how to do it better + + // If control mode is rates set _rates_sp + if(_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + _rates_sp.roll = _offboard.p1; + _rates_sp.pitch = _offboard.p2; + _rates_sp.yaw = _offboard.p3; + _rates_sp.roll = _offboard.p4; + + // If control mode is atitude set _att_sp + } else if (_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + _control_mode.flag_control_attitude_enabled = true; + + + /* pass throttle directly if not in altitude control mode */ + if (!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = _offboard.p4; + } + + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + _att_sp.yaw_body = _att.yaw; + } + else{ + _att_sp.yaw_body = _offboard.p3; + } + + _att_sp.roll_body = _offboard.p1; + _att_sp.pitch_body = _offboard.p2; + + _att_sp.R_valid = false; + publish_att_sp = true; + } + } else { /* onboard control */ @@ -565,7 +626,7 @@ MulticopterAttitudeControl::task_main() if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); - +printf("att_sp RPYT: %.2f %.2f %.2f %.2f", _att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body, _att_sp.thrust) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); -- cgit v1.2.3 From d4ae1c01adea93f23b71e742346b2d892204716c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 12:24:04 +0100 Subject: Revert "added offboard data handling in mc_att_control_main.cpp" This reverts commit 6bb85a323ce7f152aead0c9133d86e5e900bd7ba. --- src/modules/mc_att_control/mc_att_control_main.cpp | 67 +--------------------- 1 file changed, 3 insertions(+), 64 deletions(-) (limited to 'src/modules') 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 129e5df8f..c4c6c0138 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -119,7 +118,6 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ - int _offboard_sub; /**< notification of offboard control updates */ int _arming_sub; /**< arming status of outputs */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -129,7 +127,6 @@ private: struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct offboard_control_setpoint_s _offboard; /**< offboard data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -176,11 +173,6 @@ private: * Check for changes in manual inputs. */ void vehicle_manual_poll(); - - /** - * Check for changes in offboard inputs. - */ - void vehicle_offboard_poll(); /** * Check for set triplet updates. @@ -347,23 +339,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } -void -MulticopterAttitudeControl::vehicle_offboard_poll() -{ - bool offboard_updated; - - /* get offboard inputs */ - orb_check(_offboard_sub, &offboard_updated); - - if (offboard_updated) { - - orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); - } - -} - - - void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -388,8 +363,6 @@ MulticopterAttitudeControl::arming_status_poll() } } - - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -503,49 +476,15 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - vehicle_offboard_poll(); float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode // but it's not very beautiful, need to think how to do it better - - // If control mode is rates set _rates_sp - if(_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - _rates_sp.roll = _offboard.p1; - _rates_sp.pitch = _offboard.p2; - _rates_sp.yaw = _offboard.p3; - _rates_sp.roll = _offboard.p4; - - // If control mode is atitude set _att_sp - } else if (_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - _control_mode.flag_control_attitude_enabled = true; - - - /* pass throttle directly if not in altitude control mode */ - if (!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _offboard.p4; - } - - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - _att_sp.yaw_body = _att.yaw; - } - else{ - _att_sp.yaw_body = _offboard.p3; - } - - _att_sp.roll_body = _offboard.p1; - _att_sp.pitch_body = _offboard.p2; - - _att_sp.R_valid = false; - publish_att_sp = true; - } - } else { /* onboard control */ @@ -626,7 +565,7 @@ MulticopterAttitudeControl::task_main() if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); -printf("att_sp RPYT: %.2f %.2f %.2f %.2f", _att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body, _att_sp.thrust) + if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); -- cgit v1.2.3 From 0877fcba0312cf859b2a8ccd9eb3b6d56ecaf7e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:52:02 +0100 Subject: offboard setpoint support --- src/modules/commander/commander.cpp | 213 ++++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 48 +++++-- src/modules/navigator/navigator_main.cpp | 57 ++++++++- 3 files changed, 215 insertions(+), 103 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b69f2b76..fa815bdfe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define OFFBOARD_TIMEOUT 200000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -699,9 +700,9 @@ int commander_thread_main(int argc, char *argv[]) memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -815,10 +816,10 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); } orb_check(sensor_sub, &updated); @@ -1040,124 +1041,162 @@ int commander_thread_main(int argc, char *argv[]) } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); - status_changed = true; + /* start RC input check */ + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; } + } - status.rc_signal_lost = false; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * 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) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * 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) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - } else { - stick_off_counter++; - } + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { + stick_off_counter = 0; + } - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + } else if (res == TRANSITION_DENIED) { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* evaluate the main state machine */ + res = check_main_state_machine(&status); - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); + if (res == TRANSITION_CHANGED) { + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { + res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + } } + } + } + + /* check offboard signal */ + if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (!status.offboard_control_signal_found_once) { + status.offboard_control_signal_found_once = true; + mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); + status_changed = true; } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; + if (status.offboard_control_signal_lost) { + mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained"); status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); + } + + status.offboard_control_signal_lost = false; + + if (status.main_state == MAIN_STATE_OFFBOARD) { + if (offboard_sp.armed && !armed.armed) { + if (!safety.safety_switch_available || safety.safety_off) { + transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); + } } } + + } else { + if (!status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); + status.offboard_control_signal_lost = true; + status_changed = true; + } } /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ @@ -1264,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_sp_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2..62597d91a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -97,6 +97,7 @@ static mavlink_status_t status; static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; +static struct vehicle_attitude_setpoint_s att_sp; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t att_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg) /* Handle quadrotor motor setpoints */ if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint); if (mavlink_system.sysid < 4) { @@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg) uint8_t ml_mode = 0; bool ml_armed = false; - switch (quad_motors_setpoint.mode) { + switch (quad_swarm_setpoint.mode) { case 0: ml_armed = false; break; @@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg) offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { - /* Publish */ + /* publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + + if (v_status.main_state == MAIN_STATE_OFFBOARD) { + /* in offboard mode also publish setpoint directly */ + switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + att_sp.R_valid = false; + att_sp.q_d_valid = false; + att_sp.q_e_valid = false; + att_sp.roll_reset_integral = false; + + /* check if topic has to be advertised */ + if (att_sp_pub <= 0) { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + break; + default: + break; + } + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7285e243e..f77c48207 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -149,8 +150,9 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_sub; /**< offboard control setpoint subscription */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ @@ -162,6 +164,7 @@ private: struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -383,6 +386,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _offboard_sub(-1), /* publications */ _triplet_pub(-1), @@ -422,6 +426,7 @@ Navigator::Navigator() : memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_offboard, 0, sizeof(_offboard)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; @@ -591,6 +596,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -1523,12 +1529,49 @@ Navigator::publish_control_mode() case MAIN_STATE_OFFBOARD: _control_mode.flag_control_offboard_enabled = true; _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _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; + + switch (_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + _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 OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _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 OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + default: + _control_mode.flag_control_rates_enabled = false; + _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; + } break; default: -- cgit v1.2.3 From 3fe5e88fbe291619783d7bc271a5152d4e42bd3f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:53:55 +0100 Subject: Reverse offboard changes in mc_att_control --- src/modules/mc_att_control/mc_att_control_main.cpp | 99 ++++++++++------------ 1 file changed, 45 insertions(+), 54 deletions(-) (limited to 'src/modules') 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 c4c6c0138..245ac024b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -480,73 +480,64 @@ MulticopterAttitudeControl::task_main() float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; - if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode - // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode - // but it's not very beautiful, need to think how to do it better - - } else { - /* onboard control */ - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); _att_sp.R_valid = false; publish_att_sp = true; } + } - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; + publish_att_sp = true; + } - } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; } } else { - /* reset yaw setpoint after non-manual control */ + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ reset_yaw_sp = true; } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } if (_att_sp.R_valid) { -- cgit v1.2.3 From 6ed6c5bb1fe6cce2c2c784d5f8145c0aa03fabe7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:56:38 +0100 Subject: cammander, navigator: code style fixed --- src/modules/commander/commander.cpp | 26 +- src/modules/commander/state_machine_helper.cpp | 56 ++-- src/modules/navigator/navigator_main.cpp | 352 +++++++++++++++---------- 3 files changed, 258 insertions(+), 176 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fa815bdfe..bc8278bb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -372,6 +372,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -406,6 +407,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -454,6 +456,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + } else { mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } @@ -497,8 +500,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -519,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO @@ -891,6 +895,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; @@ -1066,9 +1071,9 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * 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) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || 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) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1086,7 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1143,14 +1148,18 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); status.set_nav_state = NAV_STATE_RTL; status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); } @@ -1178,13 +1187,16 @@ int commander_thread_main(int argc, char *argv[]) if (offboard_sp.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); } @@ -1202,9 +1214,11 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); + if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } + } else { flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e5599e1c3..4dd63b5e1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -238,8 +239,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum altitude estimate */ if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -249,7 +250,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum local position estimate */ if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -373,35 +374,36 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->flighttermination_state) { + ret = TRANSITION_NOT_CHANGED; - } else { + } else { - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; - default: - break; - } + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + default: + break; } - return ret; + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f77c48207..0e317d784 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -167,12 +167,12 @@ private: struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -369,7 +369,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -508,16 +508,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -534,6 +538,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -576,11 +581,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -597,7 +604,7 @@ Navigator::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -665,8 +672,9 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { + (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -674,21 +682,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { /* switch to mission only if available */ if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + stick_mode = true; } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); @@ -714,15 +728,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -735,6 +752,7 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } @@ -762,7 +780,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -783,6 +801,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -791,15 +810,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -849,48 +868,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); + (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -899,8 +925,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -910,7 +936,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -919,8 +945,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -929,8 +955,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -993,6 +1019,7 @@ Navigator::start_loiter() if (_global_pos.alt < min_alt_amsl) { _mission_item_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1043,27 +1070,28 @@ Navigator::set_mission_item() add_home_pos_to_rtl(&_mission_item_triplet.current); if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ float takeoff_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) takeoff_alt_amsl += _home_pos.altitude; @@ -1089,6 +1117,7 @@ Navigator::set_mission_item() _mission_item_triplet.current.altitude = takeoff_alt_amsl; _mission_item_triplet.current.altitude_is_relative = false; } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1097,13 +1126,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; @@ -1116,6 +1148,7 @@ Navigator::set_mission_item() if (ret == OK) { add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; + } else { /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; @@ -1129,17 +1162,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1150,107 +1187,112 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + + if (_vstatus.condition_landed) + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = climb_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + // don't change altitude setpoint + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + float descend_alt = _home_pos.altitude + _parameters.land_alt; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = descend_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_mission_item_triplet(); @@ -1267,6 +1309,7 @@ Navigator::check_mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1281,7 +1324,7 @@ Navigator::check_mission_item_reached() _mission_item_triplet.current.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1301,18 +1344,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) _mission_item_triplet.current.altitude += _home_pos.altitude; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1324,9 +1369,11 @@ Navigator::check_mission_item_reached() if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1336,20 +1383,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; + if (_mission_item_triplet.current.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1362,6 +1411,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1369,23 +1419,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1508,6 +1563,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1516,6 +1572,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1524,6 +1581,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } + break; case MAIN_STATE_OFFBOARD: @@ -1539,6 +1597,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1547,6 +1606,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1555,6 +1615,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = true; break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1563,6 +1624,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; break; + default: _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1572,6 +1634,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; break; } + break; default: @@ -1591,7 +1654,8 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) +{ if (a.altitude_is_relative == b.altitude_is_relative && fabs(a.lat - b.lat) < FLT_EPSILON && fabs(a.lon - b.lon) < FLT_EPSILON && @@ -1604,6 +1668,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; + } else { return false; } @@ -1664,8 +1729,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); -- cgit v1.2.3 From dfedbcb855e53da5fe2ce4d37d9f4fba54626d62 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 24 Jan 2014 23:56:54 +0000 Subject: added check for MAIN_STATE_OFFBOARD @#1152 so that we dont get thrown out of this state --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc8278bb7..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1149,7 +1149,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_CHANGED) { -- cgit v1.2.3 From a037861ec21834428416e92482feacad749e1cd3 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Mon, 27 Jan 2014 14:33:34 +0000 Subject: Added method to update _offboard in navigator_main.cpp --- src/modules/navigator/navigator_main.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e317d784..829511053 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -271,6 +271,11 @@ private: * Retrieve vehicle status */ void vehicle_status_update(); + + /** + * Retrieve offboard setpoint + */ + void offboard_update(); /** @@ -503,6 +508,11 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } +void +Navigator::offboard_update() +{ + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); +} void Navigator::offboard_mission_update(bool isrotaryWing) @@ -613,6 +623,7 @@ Navigator::task_main() navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); + offboard_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -623,7 +634,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -640,6 +651,9 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _offboard_sub; + fds[7].events = POLLIN; + while (!_task_should_exit) { @@ -825,6 +839,11 @@ Navigator::task_main() } } + /* offboard setpoint updated */ + if (fds[7].revents & POLLIN) { + offboard_update(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); -- cgit v1.2.3 From 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 29 Jan 2014 15:39:58 +0000 Subject: Added check for offboard_control_signal_lost when switching to offboard mode with rc switch --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bec6884e2..3687f55fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 01c9092213449c761759bcda11ef9613226be713 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 18:06:03 +0100 Subject: Revert "Added check for offboard_control_signal_lost when switching to offboard mode with rc switch" This reverts commit 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fb..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 8ab94150db31e270b34cecaa4701628df873042c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 20:23:23 +0100 Subject: commander: print_reject_mode() bug fixed --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 246efd61e..2c2b680b4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1624,7 +1624,7 @@ set_main_state_rc(struct vehicle_status_s *status) if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { res = main_state_transition(status, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); + print_reject_mode(status, "OFFBOARD"); } else { return res; -- cgit v1.2.3 From 15ea9cd401351f7487cff35333780d6d4ff47e09 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 14 Feb 2014 12:37:10 +0000 Subject: minor cosmetic changes --- src/modules/commander/commander.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fb..3e5d9dcf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1491,7 +1491,6 @@ check_main_state_machine(struct vehicle_status_s *current_status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { /* offboard switch overrides main switch */ res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 62597d91a..f3345b35f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,23 +288,21 @@ handle_message(mavlink_message_t *msg) ml_armed = false; break; - case 1: + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; - case 2: + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; - case 3: + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; break; - case 4: + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; break; } -- cgit v1.2.3 From 66f270dd7e2a8668ae7a4f3387e5098381eff84d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 14 Feb 2014 13:39:57 +0100 Subject: commander: double subscribing to offboard setpoint fixed --- src/modules/commander/commander.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c2b680b4..bf7bf2ef3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -761,11 +761,6 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); - /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); struct offboard_control_setpoint_s sp_offboard; @@ -883,10 +878,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); - } - orb_check(sp_offboard_sub, &updated); if (updated) { @@ -1285,7 +1276,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check offboard signal */ - if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1301,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; if (status.main_state == MAIN_STATE_OFFBOARD) { - if (offboard_sp.armed && !armed.armed) { + if (sp_offboard.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1310,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } } - } else if (!offboard_sp.armed && armed.armed) { + } else if (!sp_offboard.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); -- cgit v1.2.3 From e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 16:50:00 +0400 Subject: commander: publish vehicle_control_mode for OFFBOARD state --- src/modules/commander/commander.cpp | 32 ++++++++++++++++++++++---- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 30 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1b9ca6988..61ef63921 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -159,6 +159,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; +static struct offboard_control_setpoint_s sp_offboard; /* tasks waiting for low prio thread */ typedef enum { @@ -767,7 +768,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); /* Subscribe to global position */ @@ -1691,6 +1691,8 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = false; control_mode.flag_control_termination_enabled = false; /* set this flag when navigator should act */ @@ -1701,7 +1703,6 @@ set_control_mode() switch (status.main_state) { case MAIN_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = status.is_rotary_wing; control_mode.flag_control_attitude_enabled = status.is_rotary_wing; control_mode.flag_control_altitude_enabled = false; @@ -1712,7 +1713,6 @@ set_control_mode() case MAIN_STATE_SEATBELT: 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 = true; control_mode.flag_control_altitude_enabled = true; @@ -1723,7 +1723,6 @@ set_control_mode() case MAIN_STATE_EASY: 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 = true; control_mode.flag_control_altitude_enabled = true; @@ -1735,6 +1734,31 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + case MAIN_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_offboard_enabled = false; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + 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; + + default: + control_mode.flag_control_rates_enabled = false; + 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; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0ca64853a..735bc50da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,6 +330,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + // TODO use vehicle_control_mode.flag_control_offboard_enabled if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7cbb37cd8..5444c4ebf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -74,6 +74,7 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ + bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ -- cgit v1.2.3 From 172c82c258819c6f2d95b85b56c41015f928b282 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 17:21:43 +0400 Subject: commander: vehicle_control_mode fixes --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f82c8c35b..1ef872e39 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1736,7 +1736,7 @@ set_control_mode() case MAIN_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1776,7 +1776,6 @@ set_control_mode() case FAILSAFE_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_position_enabled = false; -- cgit v1.2.3 From 3b270fbdab60c0fac1c76dff170b0a9b6f494e8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 18:07:12 +0400 Subject: commander: auto arm/disarm by offboard thrust "feature" removed --- src/modules/commander/commander.cpp | 20 -------------------- 1 file changed, 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ef872e39..4a69318c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1295,26 +1295,6 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; - if (status.main_state == MAIN_STATE_OFFBOARD) { - if (sp_offboard.armed && !armed.armed) { - if (!safety.safety_switch_available || safety.safety_off) { - transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); - } - } - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); - } - } - } - } else { if (!status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); -- cgit v1.2.3 From 9c20f86899b17dc6cd1e7642a6212e370474345e Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Tue, 18 Feb 2014 11:10:52 +0000 Subject: lalala --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 735bc50da..b724447c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,7 +330,13 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - // TODO use vehicle_control_mode.flag_control_offboard_enabled + static struct vehicle_control_mode_s _v_control_mode; + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + + /**< vehicle control mode subscription */ + static int _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + + // TODO use vehicle_control_mode.flag_control_offboard_enabled if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { -- cgit v1.2.3 From 92e15b83cc565cab149b7a8103794672dc82dcb5 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:24:32 +0000 Subject: added subscriber to vehicle_control_mode --- src/modules/mavlink/mavlink_receiver.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b724447c5..8ea0f9ea3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -117,6 +118,8 @@ static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t pub_hil_battery = -1; +static int _v_control_mode_sub = -1; + /* packet counter */ static int hil_counter = 0; static int hil_frames = 0; @@ -330,14 +333,14 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - static struct vehicle_control_mode_s _v_control_mode; - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - - /**< vehicle control mode subscription */ - static int _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - // TODO use vehicle_control_mode.flag_control_offboard_enabled - if (v_status.main_state == MAIN_STATE_OFFBOARD) { + // TODO use vehicle_control_mode.flag_control_offboard_enabled + bool updated; + orb_check(_v_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } + if(_v_control_mode.flag_control_offboard_enabled) { +// if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -863,6 +866,10 @@ receive_thread(void *arg) ssize_t nread = 0; + // vehicle control mode subscription + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) { -- cgit v1.2.3 From faa7d9577d20d38741fe9accabb372f088b9dc6a Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:27:42 +0000 Subject: commented out gcs_link = false; when receiving swarm messages, since we still want imu and atitude messages in offboard mode --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ea0f9ea3..6c0eb169b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -277,7 +277,7 @@ handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - gcs_link = false; +// gcs_link = false; // commented this because I wanted to still receive imu and attitude info when in offboard mode /* * rate control mode - defined by MAVLink -- cgit v1.2.3 From f4a6b07e293037d632f080da4ff7504ff97731f6 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:29:31 +0000 Subject: indenting changes to make should_arm condition more readable --- src/modules/px4iofirmware/mixer.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a4429461..760ca2e27 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -173,15 +173,16 @@ mixer_tick(void) * here. */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)/* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)/* and IO is armed */ + && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)/* and FMU is armed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))/* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)/* or direct PWM is set */ + || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))/* or failsafe was set manually */ + ) + ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) -- cgit v1.2.3 From 0656aae3cb46f405888d8130e9db4bb3f9dd529a Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 14:31:23 +0000 Subject: Now setting flag_external_manual_override_ok to false when in offboard mode,so that fmu has control over outputs. Added handling of OFFBOARD_CONTROL_MODE_DIRECT_RATES. --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f0debef1c..5e242050b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,8 +1728,17 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; + control_mode.flag_external_manual_override_ok = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + 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; + control_mode.flag_external_manual_override_ok = false; break; - default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6c0eb169b..ba886e29e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_rates_setpoint_s rates_sp; static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; @@ -130,6 +131,7 @@ static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t att_sp_pub = -1; +static orb_advert_t rates_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -343,6 +345,22 @@ handle_message(mavlink_message_t *msg) // if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + /* check if topic has to be advertised */ + if(rates_sp_pub <= 0) { + rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = offboard_control_sp.p1; -- cgit v1.2.3 From be4af0ab3bd1b4313ba23cbe4c7e2702d56d3644 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 17:07:29 +0000 Subject: code clean up --- src/examples/jetdrive_control/jetdrive_control_main.cpp | 2 +- src/modules/commander/commander.cpp | 2 -- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index 409ccad63..01435f572 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -558,7 +558,7 @@ JetdriveControl::task_main() arming_status_poll(); vehicle_manual_poll(); - if(_v_control_mode.flag_external_manual_override_ok) + if(!_v_control_mode.flag_control_offboard_enabled) { /* manual/direct control */ _actuators.control[0] = _manual_control_sp .roll; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5e242050b..801ef8d01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,7 +1728,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_RATES: control_mode.flag_control_rates_enabled = true; @@ -1737,7 +1736,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ba886e29e..a834b2c3a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -358,7 +358,7 @@ handle_message(mavlink_message_t *msg) } else { /* publish */ - orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: -- cgit v1.2.3 From f4c36f8c5cf69bd3af55a1065ab7a18c999c054c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 20:57:36 +0200 Subject: mc_pos_control: use current velocity to calculate position setpoint on reset to make transition to stabilized modes more smooth --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7c625a0c5..fe0e8fe1a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -493,8 +493,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +504,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 From 7232c0354bee1f8f572bdb2f53f9969ba9778a6e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 23:01:30 +0200 Subject: mc_pos_control: use MPC_XXX_FF to adjust setpoint on reset --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe0e8fe1a..356f3c15e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -388,9 +388,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -493,8 +495,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +506,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 From 7813566e6680f4940989fb91760ddb0782b05858 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:30:45 +0400 Subject: Initial UAVCAN integration. The library compiles successfully, CAN driver appears to be working properly. There is one hardcoded path in the module makefile that needs to be fixed; plus the compilation will likely fail unless arch/math.h contains log2l() --- .gitignore | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/setup.mk | 1 + makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/.gitignore | 1 + src/modules/uavcan/module.mk | 74 +++++++++++++++++++++++++ src/modules/uavcan/uavcan_clock.cpp | 65 ++++++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 100 ++++++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 39 +++++++++++++ 9 files changed, 283 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/.gitignore create mode 100644 src/modules/uavcan/module.mk create mode 100644 src/modules/uavcan/uavcan_clock.cpp create mode 100644 src/modules/uavcan/uavcan_main.cpp create mode 100644 src/modules/uavcan/uavcan_main.hpp (limited to 'src/modules') diff --git a/.gitignore b/.gitignore index 5b345b34a..8c3bb1bb5 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject +/uavcan diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc..09b5bf7c6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,6 +73,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 183b143d6..a5271c656 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -48,6 +48,7 @@ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ +export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b519e0e7a..3c00e77f1 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -111,7 +111,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore new file mode 100644 index 000000000..24fbf171f --- /dev/null +++ b/src/modules/uavcan/.gitignore @@ -0,0 +1 @@ +./dsdlc_generated/ diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk new file mode 100644 index 000000000..5caa8d401 --- /dev/null +++ b/src/modules/uavcan/module.mk @@ -0,0 +1,74 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Author: Pavel Kirienko +# +# 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. +# +############################################################################ + +# +# UAVCAN <--> uORB bridge +# + +MODULE_COMMAND = uavcan + +MAXOPTIMIZATION = -Os + +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp + +# +# libuavcan +# +include $(UAVCAN_DIR)/libuavcan/include.mk +SRCS += $(LIBUAVCAN_SRC) +# TODO fix include path +INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ + -DUAVCAN_TOSTRING=0 \ + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ + -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + +# +# libuavcan drivers for STM32 +# +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +SRCS += $(LIBUAVCAN_STM32_SRC) +INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) +EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 + +# +# Invoke DSDL compiler +# TODO: Add make target for this, or invoke dsdlc manually. +# The second option assumes that the generated headers shall be saved +# under the version control, which may be undesirable. +# The first option requires python3 and python3-mako for the sources to be built. +# +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp new file mode 100644 index 000000000..2a5e88989 --- /dev/null +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#include +#include + +namespace uavcan_stm32 +{ +namespace clock +{ + +uavcan::MonotonicTime getMonotonic() +{ + return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); +} + +uavcan::UtcTime getUtc() +{ + return uavcan::UtcTime(); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + (void)adjustment; +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return 0; +} + +} +} + diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp new file mode 100644 index 000000000..fbb2e174b --- /dev/null +++ b/src/modules/uavcan/uavcan_main.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "uavcan_main.hpp" + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + +namespace +{ + +uavcan_stm32::CanInitHelper<> can_driver; + +void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +int test_thread(int argc, char *argv[]) +{ + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_TX); + int res = can_driver.init(1000000); + if (res < 0) + { + errx(res, "CAN driver init failed"); + } + while (true) + { + ::sleep(1); + auto iface = static_cast(can_driver.driver.getIface(0)); + res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); + warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + } + return 0; +} + +} + +int uavcan_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[1], "start")) { + static bool started = false; + if (started) + { + warnx("already started"); + ::exit(1); + } + started = true; + (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, + static_cast(&test_thread), const_cast(argv)); + return 0; + } else { + print_usage(); + ::exit(1); + } + return 0; +} diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp new file mode 100644 index 000000000..7fcc992d0 --- /dev/null +++ b/src/modules/uavcan/uavcan_main.hpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +// ... -- cgit v1.2.3 From 7d7a375dd1a69bd286f127de51b11e5ecd390640 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:42:40 +0400 Subject: Fixed hardcoded include path --- makefiles/nuttx.mk | 1 + src/modules/uavcan/module.mk | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index d283096b2..bf0744140 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # Add directories from the NuttX export to the relevant search paths # INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ + $(NUTTX_EXPORT_DIR)include/cxx \ $(NUTTX_EXPORT_DIR)arch/chip \ $(NUTTX_EXPORT_DIR)arch/common diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 5caa8d401..6974de8ca 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -48,8 +48,7 @@ SRCS += uavcan_main.cpp \ # include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) -# TODO fix include path -INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ @@ -61,7 +60,8 @@ EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 +EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ + -DUAVCAN_STM32_NUM_IFACES=1 # # Invoke DSDL compiler -- cgit v1.2.3 From 5716dad25db27315fa7cebf8183a71f864860f41 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 20:14:07 +0400 Subject: Added workaround for hardware issue on Pixhawk v1 --- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 6974de8ca..3966210b5 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -61,7 +61,7 @@ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=1 + -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fbb2e174b..474063aa6 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "uavcan_main.hpp" extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -53,9 +54,14 @@ void print_usage() int test_thread(int argc, char *argv[]) { + /* + * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. + * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to + * fail during initialization. + */ stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); int res = can_driver.init(1000000); if (res < 0) -- cgit v1.2.3 From 4b11145797fdc26e5bf29738dd319ab9e8003356 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:42:34 +0400 Subject: Working UAVCAN node. No application logic is implemented yet; the node just publishes its status once a second (uavcan.protocol.NodeStatus) and responds to basic services (transport stats, node discovery) --- src/modules/uavcan/uavcan_clock.cpp | 7 ++ src/modules/uavcan/uavcan_main.cpp | 149 ++++++++++++++++++++++++++++-------- src/modules/uavcan/uavcan_main.hpp | 29 ++++++- 3 files changed, 151 insertions(+), 34 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 2a5e88989..0e844f346 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -60,6 +60,13 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() return 0; } +} // namespace clock + +SystemClock& SystemClock::instance() +{ + static SystemClock inst; + return inst; } + } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 474063aa6..bf6d4fb08 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -40,64 +40,147 @@ #include #include "uavcan_main.hpp" -extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); +/* + * UavcanNode + */ +UavcanNode* UavcanNode::_instance; -namespace +int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } -uavcan_stm32::CanInitHelper<> can_driver; - -void print_usage() -{ - warnx("usage: uavcan start [can_bitrate]"); -} - -int test_thread(int argc, char *argv[]) -{ /* + * GPIO config. * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio(GPIO_CAN1_RX); - stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio (GPIO_CAN1_RX); + stm32_configgpio (GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio(GPIO_CAN2_TX); - int res = can_driver.init(1000000); - if (res < 0) - { - errx(res, "CAN driver init failed"); + stm32_configgpio (GPIO_CAN2_TX); + + /* + * CAN driver init + */ + static CanInitHelper can; + static bool can_initialized = false; + if (!can_initialized) { + const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { + warnx("CAN driver init failed %i", can_init_res); + return can_init_res; + } + can_initialized = true; } - while (true) - { - ::sleep(1); - auto iface = static_cast(can_driver.driver.getIface(0)); - res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); - warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + + /* + * Node init + */ + _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; } - return 0; + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { + delete _instance; + _instance = nullptr; + warnx("Node init failed %i", node_init_res); + return node_init_res; + } + + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast(run_trampoline), nullptr); } +int UavcanNode::init(uavcan::NodeID node_id) +{ + uavcan::protocol::SoftwareVersion swver; + swver.major = 12; // TODO fill version info + swver.minor = 34; + _node.setSoftwareVersion(swver); + + uavcan::protocol::HardwareVersion hwver; + hwver.major = 42; // TODO fill version info + hwver.minor = 42; + _node.setHardwareVersion(hwver); + + _node.setName("org.pixhawk"); // Huh? + + _node.setNodeID(node_id); + + return _node.start(); +} + +int UavcanNode::run() +{ + _node.setStatusOk(); + while (true) { + // TODO: ORB multiplexing + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { + warnx("Spin error %i", res); + ::sleep(1); + } + } + return -1; } +/* + * App entry point + */ +static void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + int uavcan_main(int argc, char *argv[]) { + constexpr long DEFAULT_CAN_BITRATE = 1000000; + if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - static bool started = false; - if (started) - { - warnx("already started"); + if (argc < 3) { + print_usage(); ::exit(1); } - started = true; - (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, - static_cast(&test_thread), const_cast(argv)); - return 0; + /* + * Node ID + */ + const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + warnx("Invalid Node ID %i", node_id); + ::exit(1); + } + /* + * CAN bitrate + */ + long bitrate = 0; + if (argc > 3) { + bitrate = atol(argv[3]); + } + if (bitrate <= 0) { + bitrate = DEFAULT_CAN_BITRATE; + } + /* + * Start + */ + warnx("Node ID %i, bitrate %li", node_id, bitrate); + return UavcanNode::start(node_id, bitrate); } else { print_usage(); ::exit(1); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 7fcc992d0..8bd660cc5 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,4 +36,31 @@ #include -// ... +/** + * Implements basic functinality of UAVCAN node. + */ +class UavcanNode +{ + static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned RxQueueLenPerIface = 64; + static constexpr unsigned StackSize = 3000; + +public: + typedef uavcan::Node Node; + typedef uavcan_stm32::CanInitHelper CanInitHelper; + + UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + : _node(can_driver, system_clock) + { } + + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } + +private: + int init(uavcan::NodeID node_id); + int run(); + + static UavcanNode* _instance; + Node _node; +}; -- cgit v1.2.3 From 04df4270f0ac6aec360076185363338366475166 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:56:05 +0400 Subject: Removed the placement new workaround. It seems like we can pull from the toolchain's standard includes with no harm. --- src/modules/uavcan/module.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3966210b5..c91208342 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -51,8 +51,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ - -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 -- cgit v1.2.3 From be728d189e4b1b04de5a0b45710e08effcf50b8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 14:24:40 +0400 Subject: Catching up with libuavcan - some preprocessor symbols are no longer required to be defined explicitly --- src/modules/uavcan/module.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index c91208342..098543879 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) -EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ - -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile +# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. +EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 -- cgit v1.2.3 From 973b193261fab69320f25fae68345a60d7678dc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:29:30 +0200 Subject: Fixed comments and code style of UAVCAN node --- src/modules/uavcan/uavcan_clock.cpp | 13 +++++++++--- src/modules/uavcan/uavcan_main.cpp | 40 +++++++++++++++++++++++++++++-------- src/modules/uavcan/uavcan_main.hpp | 19 ++++++++++++------ 3 files changed, 55 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 0e844f346..e41d5f953 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -35,6 +34,14 @@ #include #include +/** + * @file uavcan_clock.cpp + * + * Implements a clock for the CAN node. + * + * @author Pavel Kirienko + */ + namespace uavcan_stm32 { namespace clock @@ -62,7 +69,7 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() } // namespace clock -SystemClock& SystemClock::instance() +SystemClock &SystemClock::instance() { static SystemClock inst; return inst; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bf6d4fb08..8d715e3b1 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -40,10 +39,18 @@ #include #include "uavcan_main.hpp" +/** + * @file uavcan_main.cpp + * + * Implements basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + /* * UavcanNode */ -UavcanNode* UavcanNode::_instance; +UavcanNode *UavcanNode::_instance; int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { @@ -58,22 +65,25 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio (GPIO_CAN1_RX); - stm32_configgpio (GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio (GPIO_CAN2_TX); + stm32_configgpio(GPIO_CAN2_TX); /* * CAN driver init */ static CanInitHelper can; static bool can_initialized = false; + if (!can_initialized) { const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { warnx("CAN driver init failed %i", can_init_res); return can_init_res; } + can_initialized = true; } @@ -81,11 +91,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Node init */ _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { warnx("Out of memory"); return -1; } + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { delete _instance; _instance = nullptr; @@ -96,9 +109,9 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) /* * Start the task. Normally it should never exit. */ - static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); } int UavcanNode::init(uavcan::NodeID node_id) @@ -123,14 +136,17 @@ int UavcanNode::init(uavcan::NodeID node_id) int UavcanNode::run() { _node.setStatusOk(); + while (true) { // TODO: ORB multiplexing const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { warnx("Spin error %i", res); ::sleep(1); } } + return -1; } @@ -158,32 +174,40 @@ int uavcan_main(int argc, char *argv[]) print_usage(); ::exit(1); } + /* * Node ID */ const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } + /* * CAN bitrate */ long bitrate = 0; + if (argc > 3) { bitrate = atol(argv[3]); } + if (bitrate <= 0) { bitrate = DEFAULT_CAN_BITRATE; } + /* * Start */ warnx("Node ID %i, bitrate %li", node_id, bitrate); return UavcanNode::start(node_id, bitrate); + } else { print_usage(); ::exit(1); } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8bd660cc5..b27449f1f 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -37,7 +36,15 @@ #include /** - * Implements basic functinality of UAVCAN node. + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + +/** + * A UAVCAN node. */ class UavcanNode { @@ -49,18 +56,18 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _node(can_driver, system_clock) { } static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node &getNode() { return _node; } private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode* _instance; + static UavcanNode *_instance; ///< pointer to the library instance Node _node; }; -- cgit v1.2.3 From ab5e76e3d9da5a76d6520e166d5370c6bdfc4a53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 15:03:08 +0200 Subject: Fixed printing of bit rate --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8d715e3b1..ca4da1c2d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -162,7 +162,7 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr long DEFAULT_CAN_BITRATE = 1000000; + constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; if (argc < 2) { print_usage(); @@ -188,7 +188,7 @@ int uavcan_main(int argc, char *argv[]) /* * CAN bitrate */ - long bitrate = 0; + unsigned bitrate = 0; if (argc > 3) { bitrate = atol(argv[3]); @@ -201,7 +201,7 @@ int uavcan_main(int argc, char *argv[]) /* * Start */ - warnx("Node ID %i, bitrate %li", node_id, bitrate); + warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); } else { -- cgit v1.2.3 From d62f3b8aa7a1ba932b932b26068b79bd14e205dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 09:14:23 +0200 Subject: Added mixing code, not complete, not compiliing --- src/modules/uavcan/uavcan_main.cpp | 254 ++++++++++++++++++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 27 +++- 2 files changed, 273 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ca4da1c2d..94fb15544 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,9 @@ #include #include #include + +#include + #include "uavcan_main.hpp" /** @@ -52,6 +55,20 @@ */ UavcanNode *UavcanNode::_instance; +UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + _node(can_driver, system_clock), + _controls({}), + _poll_fds({}) +{ + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); +} + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -106,6 +123,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } + int ret; + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + /* * Start the task. Normally it should never exit. */ @@ -137,8 +162,94 @@ int UavcanNode::run() { _node.setStatusOk(); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + while (true) { - // TODO: ORB multiplexing + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } + + int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); + + } else { + + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } + + /* can we mix? */ + if (_mixers != nullptr) { + + // XXX one output group has 8 outputs max, + // but this driver could well serve multiple groups. + unsigned num_outputs_max = 8; + + // Do mixing + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (!isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * 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 + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + printf("CAN out: ") + /* output to the bus */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + printf("%u: %8.4f ", i, outputs.output[i]) + // can_send_xxx + } + printf("\n"); + + } + } + + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + arm_actuators(set_armed); + } + + // Output commands and fetch data + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); if (res < 0) { @@ -147,7 +258,146 @@ int UavcanNode::run() } } - return -1; + teardown(); + + exit(0); +} + +int +UavcanNode::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = _controls[control_group].control[control_index]; + return 0; +} + +int +UavcanNode::teardown() +{ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); +} + +void +UavcanNode::arm_actuators(bool arm) +{ + bool changed = (_armed != arm); + + _armed = arm; + + if (changed) { + // Propagate immediately to CAN bus + } +} + +void +UavcanNode::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + arm_actuators(true); + break; + + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + // these are no-ops, as no safety switch + break; + + case PWM_SERVO_DISARM: + arm_actuators(false); + break; + + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _output_count; + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; } /* diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b27449f1f..213cb4206 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include /** * @file uavcan_main.hpp @@ -46,7 +47,7 @@ /** * A UAVCAN node. */ -class UavcanNode +class UavcanNode : public device::CDev { static constexpr unsigned MemPoolSize = 10752; static constexpr unsigned RxQueueLenPerIface = 64; @@ -56,18 +57,32 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) - : _node(can_driver, system_clock) - { } + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); static int start(uavcan::NodeID node_id, uint32_t bitrate); Node &getNode() { return _node; } + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + void subscribe(); + private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< pointer to the library instance + Node _node; + + MixerGroup *_mixers; + + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; }; -- cgit v1.2.3 From 517f2df0d1de008c795061badc57fbfdbb68d0d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:28:41 +0200 Subject: UAVCAN: Fixed all compile errors --- src/modules/uavcan/uavcan_main.cpp | 124 ++++++++++++++++++++++++++----------- src/modules/uavcan/uavcan_main.hpp | 46 ++++++++++---- 2 files changed, 123 insertions(+), 47 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 94fb15544..332c3a5e8 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,14 +31,18 @@ * ****************************************************************************/ +#include + #include #include #include #include +#include #include #include -#include +#include +#include #include "uavcan_main.hpp" @@ -55,7 +59,13 @@ */ UavcanNode *UavcanNode::_instance; -UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : +UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + CDev("uavcan", UAVCAN_DEVICE_PATH), + _task(0), + _task_should_exit(false), + _armed_sub(-1), + _is_armed(false), + _output_count(0), _node(can_driver, system_clock), _controls({}), _poll_fds({}) @@ -65,8 +75,37 @@ UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); + // memset(_controls, 0, sizeof(_controls)); + // memset(_poll_fds, 0, sizeof(_poll_fds)); +} + +UavcanNode::~UavcanNode() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + ::close(_armed_sub); + + _instance = nullptr; } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -123,24 +162,32 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } - int ret; - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) - return ret; - /* * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, static_cast(run_trampoline), nullptr); + + if (_instance->_task < 0) { + warnx("start failed: %d", errno); + return -errno; + } + + return OK; } int UavcanNode::init(uavcan::NodeID node_id) { + + int ret; + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -162,32 +209,35 @@ int UavcanNode::run() { _node.setStatusOk(); + // XXX figure out the output count + _output_count = 2; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - while (true) { - + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + + while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; } int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); - /* this would be bad... */ + // this would be bad... if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ -// warnx("no PWM: failsafe"); + // timeout: no control data, switch to failsafe values } else { - /* get controls for required topics */ + // get controls for required topics unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -198,7 +248,7 @@ int UavcanNode::run() } } - /* can we mix? */ + //can we mix? if (_mixers != nullptr) { // XXX one output group has 8 outputs max, @@ -209,9 +259,9 @@ int UavcanNode::run() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ + // last resort: catch NaN, INF and out-of-band errors if (!isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || outputs.output[i] > 1.0f) { @@ -224,25 +274,25 @@ int UavcanNode::run() } } - printf("CAN out: ") + printf("CAN out: "); /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]) + printf("%u: %8.4f ", i, outputs.output[i]); // can_send_xxx } - printf("\n"); + printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } } - /* check arming state */ + // Check arming state bool updated = false; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ + // Update the armed status and check that we're not locked down bool set_armed = _armed.armed && !_armed.lockdown; arm_actuators(set_armed); @@ -271,7 +321,7 @@ UavcanNode::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = _controls[control_group].control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -287,22 +337,24 @@ UavcanNode::teardown() ::close(_armed_sub); } -void +int UavcanNode::arm_actuators(bool arm) { - bool changed = (_armed != arm); + bool changed = (_is_armed != arm); - _armed = arm; + _is_armed = arm; if (changed) { // Propagate immediately to CAN bus } + + return OK; } void UavcanNode::subscribe() { - /* subscribe/unsubscribe to required actuator control groups */ + // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; @@ -326,7 +378,7 @@ UavcanNode::subscribe() } int -PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 213cb4206..857c1dc66 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -33,8 +33,14 @@ #pragma once +#include + #include -#include +#include + +#include +#include +#include /** * @file uavcan_main.hpp @@ -44,6 +50,9 @@ * @author Pavel Kirienko */ +#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 +#define UAVCAN_DEVICE_PATH "/dev/uavcan" + /** * A UAVCAN node. */ @@ -57,21 +66,36 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + + virtual ~UavcanNode(); - static int start(uavcan::NodeID node_id, uint32_t bitrate); + static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node &getNode() { return _node; } + Node& getNode() { return _node; } static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - void subscribe(); + + void subscribe(); + + int teardown(); + int arm_actuators(bool arm); private: - int init(uavcan::NodeID node_id); - int run(); + int init(uavcan::NodeID node_id); + int run(); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + int _task; ///< handle to the OS task + bool _task_should_exit; ///< flag to indicate to tear down the CAN driver + int _armed_sub; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed; ///< the arming status of the actuators on the bus + + unsigned _output_count; ///< number of actuators currently available static UavcanNode *_instance; ///< pointer to the library instance Node _node; @@ -80,9 +104,9 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; unsigned _poll_fds_num; }; -- cgit v1.2.3 From 185c95fda6acac869c1821846d44359faeef22d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:57:23 +0200 Subject: UAVCAN: improve printing, ready for full closed loop test --- src/modules/uavcan/uavcan_main.cpp | 46 +++++++++++++++++++++++++++++++++----- src/modules/uavcan/uavcan_main.hpp | 9 ++++++-- 2 files changed, 47 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 332c3a5e8..a86314852 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -68,7 +68,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _output_count(0), _node(can_driver, system_clock), _controls({}), - _poll_fds({}) + _poll_fds({}), + _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), + _poll_fds_num(0) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -225,7 +229,7 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } - int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); // this would be bad... if (ret < 0) { @@ -234,6 +238,7 @@ int UavcanNode::run() } else if (ret == 0) { // timeout: no control data, switch to failsafe values + // XXX trigger failsafe } else { @@ -278,11 +283,12 @@ int UavcanNode::run() /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // can_send_xxx + // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } + } // Check arming state @@ -300,7 +306,7 @@ int UavcanNode::run() // Output commands and fetch data - const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); if (res < 0) { warnx("Spin error %i", res); @@ -309,6 +315,7 @@ int UavcanNode::run() } teardown(); + warnx("exiting."); exit(0); } @@ -378,7 +385,7 @@ UavcanNode::subscribe() } int -UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; @@ -428,7 +435,7 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + warnx("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -449,9 +456,24 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) unlock(); + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + return ret; } +void +UavcanNode::print_info() +{ + if (!_instance) { + warnx("not running, start first"); + } + + warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); +} + /* * App entry point */ @@ -511,5 +533,17 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + /* commands below require the app to be started */ + UavcanNode *inst = UavcanNode::instance(); + + if (!inst) { + errx(1, "application not running"); + } + + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { + + inst->print_info(); + } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 857c1dc66..97598ddf3 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -66,10 +66,12 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); virtual ~UavcanNode(); + virtual int ioctl(file *filp, int cmd, unsigned long arg); + static int start(uavcan::NodeID node_id, uint32_t bitrate); Node& getNode() { return _node; } @@ -84,10 +86,13 @@ public: int teardown(); int arm_actuators(bool arm); + void print_info(); + + static UavcanNode* instance() { return _instance; } + private: int init(uavcan::NodeID node_id); int run(); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); int _task; ///< handle to the OS task bool _task_should_exit; ///< flag to indicate to tear down the CAN driver -- cgit v1.2.3 From f70db56e90972cf0492ac9d295c3d0f5df87aa66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 14:14:52 +0200 Subject: UAVCAN: Fix start / stop commands --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a86314852..9cd486bf5 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -92,8 +92,8 @@ UavcanNode::~UavcanNode() unsigned i = 10; do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); + /* wait 5ms - it should wake every 10ms or so worst-case */ + usleep(5000); /* if we have given up, kill it */ if (--i == 0) { @@ -522,15 +522,16 @@ int uavcan_main(int argc, char *argv[]) bitrate = DEFAULT_CAN_BITRATE; } + if (UavcanNode::instance()) { + errx(1, "already started"); + } + /* * Start */ warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } else { - print_usage(); - ::exit(1); } /* commands below require the app to be started */ @@ -543,7 +544,16 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); + return OK; } - return 0; + if (!std::strcmp(argv[1], "stop")) { + + delete inst; + inst = nullptr; + return OK; + } + + print_usage(); + ::exit(1); } -- cgit v1.2.3 From 4055833c9e6b6ecadab44d231047c44796ff17bc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:03:40 +0400 Subject: UAVCAN mixer renamed to /dev/uavcan/esc --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d6f1b54bc..da4c73470 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -27,7 +27,7 @@ then if [ $OUTPUT_MODE == can ] then - set OUTPUT_DEV /dev/uavcan + set OUTPUT_DEV /dev/uavcan/esc fi if mixer load $OUTPUT_DEV $MIXER_FILE diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 97598ddf3..beaa5e1d4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -51,7 +51,7 @@ */ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 -#define UAVCAN_DEVICE_PATH "/dev/uavcan" +#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" /** * A UAVCAN node. -- cgit v1.2.3 From f4c28473f9038875a56eace4b9b7364694bb03df Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:12:05 +0400 Subject: Warning fixes --- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9cd486bf5..859db93c7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -336,12 +336,12 @@ int UavcanNode::teardown() { for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } - ::close(_armed_sub); + return ::close(_armed_sub); } int -- cgit v1.2.3 From 4a98dae227f3e60f1a220164bce0b995ce303f3d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 19:42:20 +0400 Subject: UAVCAN ESC controller - proof of concept state --- src/modules/uavcan/esc_controller.cpp | 124 ++++++++++++++++++++++++++++++++++ src/modules/uavcan/esc_controller.hpp | 99 +++++++++++++++++++++++++++ src/modules/uavcan/module.mk | 5 +- src/modules/uavcan/uavcan_main.cpp | 47 ++++++------- src/modules/uavcan/uavcan_main.hpp | 35 +++++----- 5 files changed, 265 insertions(+), 45 deletions(-) create mode 100644 src/modules/uavcan/esc_controller.cpp create mode 100644 src/modules/uavcan/esc_controller.hpp (limited to 'src/modules') diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp new file mode 100644 index 000000000..bde4d2a7f --- /dev/null +++ b/src/modules/uavcan/esc_controller.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 esc_controller.cpp + * + * @author Pavel Kirienko + */ + +#include "esc_controller.hpp" +#include + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +int UavcanEscController::init() +{ + int res = -1; + + // ESC status subscription + res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + assert(outputs != nullptr); + assert(num_outputs <= MAX_ESCS); + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + + assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); + assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); + + msg.cmd.push_back(scaled); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp new file mode 100644 index 000000000..0ed0c59b5 --- /dev/null +++ b/src/modules/uavcan/esc_controller.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 esc_controller.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::TimerEventForwarder _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; +}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 098543879..ce9f981a8 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,8 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp # # libuavcan diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 859db93c7..e7829cbba 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -61,18 +61,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _task(0), - _task_should_exit(false), - _armed_sub(-1), - _is_armed(false), - _output_count(0), _node(can_driver, system_clock), - _controls({}), - _poll_fds({}), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _poll_fds_num(0) + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -183,8 +173,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) int UavcanNode::init(uavcan::NodeID node_id) { - - int ret; + int ret = -1; /* do regular cdev init */ ret = CDev::init(); @@ -192,6 +181,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + ret = _esc_controller.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -222,6 +215,11 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + /* + * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); + * IO multiplexing shall be done here. + */ + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { @@ -279,14 +277,16 @@ int UavcanNode::run() } } + /* + * Output to the bus + */ printf("CAN out: "); - /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); + _esc_controller.update_outputs(outputs.output, outputs.noutputs); } } @@ -304,13 +304,12 @@ int UavcanNode::run() arm_actuators(set_armed); } - // Output commands and fetch data + // Output commands and fetch data TODO ORB multiplexing - const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); + const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - if (res < 0) { - warnx("Spin error %i", res); - ::sleep(1); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); } } @@ -347,14 +346,8 @@ UavcanNode::teardown() int UavcanNode::arm_actuators(bool arm) { - bool changed = (_is_armed != arm); - _is_armed = arm; - - if (changed) { - // Propagate immediately to CAN bus - } - + _esc_controller.arm_esc(arm); return OK; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index beaa5e1d4..f4a709c79 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,8 @@ #include #include +#include "esc_controller.hpp" + /** * @file uavcan_main.hpp * @@ -94,24 +96,25 @@ private: int init(uavcan::NodeID node_id); int run(); - int _task; ///< handle to the OS task - bool _task_should_exit; ///< flag to indicate to tear down the CAN driver - int _armed_sub; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system - bool _is_armed; ///< the arming status of the actuators on the bus + int _task = -1; ///< handle to the OS task + bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + int _armed_sub = -1; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed = false; ///< the arming status of the actuators on the bus - unsigned _output_count; ///< number of actuators currently available + unsigned _output_count = 0; ///< number of actuators currently available - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< singleton pointer + Node _node; ///< library instance + UavcanEscController _esc_controller; - MixerGroup *_mixers; + MixerGroup *_mixers = nullptr; - uint32_t _groups_required; - uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - unsigned _poll_fds_num; + uint32_t _groups_required = 0; + uint32_t _groups_subscribed = 0; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + unsigned _poll_fds_num = 0; }; -- cgit v1.2.3 From c697aae17a32f25b2f163282b9cb18efedb14d77 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 23:34:23 +0400 Subject: Proper IO miltiplexing libuavcan + ORB --- src/modules/uavcan/uavcan_main.cpp | 68 +++++++++++++++++++++++--------------- src/modules/uavcan/uavcan_main.hpp | 3 +- 2 files changed, 44 insertions(+), 27 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e7829cbba..6ba596ad0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -202,9 +203,17 @@ int UavcanNode::init(uavcan::NodeID node_id) return _node.start(); } +void UavcanNode::node_spin_once() +{ + const int spin_res = _node.spin(uavcan::MonotonicTime()); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } +} + int UavcanNode::run() { - _node.setStatusOk(); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count _output_count = 2; @@ -215,42 +224,65 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + if (busevent_fd < 0) + { + warnx("Failed to open %s", uavcan_stm32::Event::DevName); + _task_should_exit = true; + } + /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ + _node.setStatusOk(); + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds_num += 1; } - int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + + node_spin_once(); // Non-blocking // this would be bad... - if (ret < 0) { + if (poll_ret < 0) { log("poll error %d", errno); continue; - - } else if (ret == 0) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } else { - // get controls for required topics + bool controls_updated = false; unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { + controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } + if (!controls_updated) { + // timeout: no control data, switch to failsafe values + // XXX trigger failsafe + } + //can we mix? if (_mixers != nullptr) { @@ -277,15 +309,7 @@ int UavcanNode::run() } } - /* - * Output to the bus - */ - printf("CAN out: "); - for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]); - } - printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); - + // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } @@ -303,14 +327,6 @@ int UavcanNode::run() arm_actuators(set_armed); } - - // Output commands and fetch data TODO ORB multiplexing - - const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - - if (spin_res < 0) { - warnx("node spin error %i", spin_res); - } } teardown(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index f4a709c79..751a94a8a 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: private: int init(uavcan::NodeID node_id); + void node_spin_once(); int run(); int _task = -1; ///< handle to the OS task @@ -115,6 +116,6 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; }; -- cgit v1.2.3 From 5a905825675a33b210469eff96f0b82a8bd70eb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:18:45 +0400 Subject: Catching up with STM32 driver optimizations in libuavcan --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6ba596ad0..d4a0d894a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -224,10 +224,10 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::Event::DevName); + warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -252,7 +252,7 @@ int UavcanNode::run() */ _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; } -- cgit v1.2.3 From 8501158427d7cf96b125eafe48193f654c7fb2f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:23:52 +0400 Subject: Micro optimization in UAVCAN polling loop --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index d4a0d894a..73f519fa1 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -284,7 +284,7 @@ int UavcanNode::run() } //can we mix? - if (_mixers != nullptr) { + if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. -- cgit v1.2.3 From 4edc432f399297ed6e74685408e80c0640873099 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:24:46 +0400 Subject: Removed misleading comment --- src/modules/uavcan/uavcan_main.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 73f519fa1..ab687a6b9 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -245,7 +245,6 @@ int UavcanNode::run() _groups_subscribed = _groups_required; /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. * Please note that with such multiplexing it is no longer possible to rely only on * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). -- cgit v1.2.3 From 559c62b6bc4923854e106be5987db92197e0bae1 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Fri, 9 May 2014 15:17:38 -0700 Subject: Changed low threshold in px4io firmware to 10% to ensure compatibility with user configured single channel, mode switches --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 56c5aa005..a8c560d40 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -47,8 +47,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 5000 -#define RC_CHANNEL_LOW_THRESH -5000 +#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ +#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -- cgit v1.2.3 From 78637ff74b4c12edca0fac9ea8eb65f38993b49f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 21 May 2014 11:14:06 +0200 Subject: mavlink: publish attitude / rates setpoint in offboard control mode --- src/modules/mavlink/mavlink_receiver.cpp | 72 +++++++++++++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 7 ++++ 2 files changed, 74 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..561744755 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,16 +102,21 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _control_mode_sub(-1), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0) { + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + memset(&_control_mode, 0, sizeof(_control_mode)); } MavlinkReceiver::~MavlinkReceiver() @@ -349,33 +354,33 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - uint8_t ml_mode = 0; + enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; bool ml_armed = false; switch (quad_motors_setpoint.mode) { case 0: - ml_armed = false; break; case 1: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + ml_armed = true; break; case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; break; + default: + break; } offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; @@ -388,7 +393,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast(ml_mode); + offboard_control_sp.mode = ml_mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -398,6 +403,63 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + if (_control_mode.flag_control_offboard_enabled) { + if (_control_mode.flag_control_position_enabled) { + // TODO + + } else if (_control_mode.flag_control_velocity_enabled) { + // TODO + + } else if (_control_mode.flag_control_attitude_enabled) { + /* attitude control */ + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + + att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + + } else if (_control_mode.flag_control_rates_enabled) { + /* rates control */ + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + + } else { + /* direct control */ + // TODO + } + } + } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..dc31b4c5a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -36,6 +36,7 @@ * MAVLink 1.0 uORB listener definition * * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -44,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +122,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -134,10 +137,14 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _att_sp_pub; + orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + + int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; -- cgit v1.2.3 From 9bad828bc0033c7017978de2321d3a8698c0afc6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 14:30:25 +0200 Subject: U-blox driver rework, step 2 Moved satellite info from vehicle_gps_position_s into a new uORB topic satellite_info. Renamed satellites_visible to satellites_used to reflect true content. sdlog2 will log info for GPS satellites only for now. --- src/drivers/blinkm/blinkm.cpp | 8 +-- src/drivers/gps/gps.cpp | 2 +- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/ubx.cpp | 3 +- src/drivers/hott/messages.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sdlog2/sdlog2.c | 48 ++++++++------ src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/satellite_info.h | 86 ++++++++++++++++++++++++++ src/modules/uORB/topics/vehicle_gps_position.h | 9 +-- 12 files changed, 130 insertions(+), 40 deletions(-) create mode 100644 src/modules/uORB/topics/satellite_info.h (limited to 'src/modules') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..39bf1fcfc 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -559,13 +559,7 @@ BlinkM::led() } /* get number of used satellites in navigation */ - num_of_used_sats = 0; - - for(unsigned satloop=0; satloopget_position_update_rate()); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 680f00d97..7b957e55d 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet) _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; + _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d7e9454ae..462875b6c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include "ubx.h" @@ -451,7 +452,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_visible = packet->numSV; + _gps_position->satellites_used = packet->numSV; ret = 1; break; diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1e779e8dc..086132573 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - msg.gps_num_sat = gps.satellites_visible; + msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..146e2d4b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -548,7 +548,7 @@ protected: cm_uint16_from_m_float(gps->epv_m), gps->vel_m_s * 100.0f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->satellites_used); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cf..46da1a0be 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -682,9 +682,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..4ff6c30aa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -139,6 +140,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + 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 */ @@ -941,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct estimator_status_report estimator_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; } buf; memset(&buf, 0, sizeof(buf)); @@ -1000,6 +1004,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -1017,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1053,6 +1059,9 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; + /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1115,14 +1124,6 @@ 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; @@ -1135,44 +1136,55 @@ 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.sats = buf_gps_pos.satellites_used; 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); + } + + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { - 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 sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.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 */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + snr_mean = 0.0f; - int satindex = buf_gps_pos.satellite_prn[i] - 1; + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { + + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ 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]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; 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++) { + for (unsigned i = 0; i < sat_info_count; i++) { /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ 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]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..44b6debc7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -40,6 +40,7 @@ #include "topics/parameter_update.h" #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" +#include "topics/satellite_info.h" #include "topics/sensor_combined.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" @@ -88,6 +89,7 @@ T Subscription::getData() { template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..fc12b9ed5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/satellite_info.h" +ORB_DEFINE(satellite_info, struct satellite_info_s); + #include "topics/home_position.h" ORB_DEFINE(home_position, struct home_position_s); diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..5f799eda4 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author 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 satellite_info.h + * Definition of the GNSS satellite info uORB topic. + */ + +#ifndef TOPIC_SAT_INFO_H_ +#define TOPIC_SAT_INFO_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite information */ + uint8_t count; /**< Number of satellites in satellite_...[] arrays */ + uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ +}; + +/** + * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs + * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf + * + * GPS 1-32 + * SBAS 120-158 + * Galileo 211-246 + * BeiDou 159-163, 33-64 + * QZSS 193-197 + * GLONASS 65-96, 255 + * + */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(satellite_info); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 5924a324d..a50f02139 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -82,14 +82,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - 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]; /**< 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 */ + uint8_t satellites_used; /**< Number of satellites used */ }; /** -- cgit v1.2.3 From 5c536ae46ec44b75fea745dd79f20b0aef2c8f16 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Mon, 2 Jun 2014 23:10:03 -0400 Subject: Added processing of postion_control offboard messages (quad_swarm) --- src/modules/mavlink/mavlink_receiver.cpp | 17 ++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 17 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 561744755..78c50c422 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,6 +102,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _local_pos_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _vicon_position_pub(-1), @@ -411,7 +412,21 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message if (_control_mode.flag_control_offboard_enabled) { if (_control_mode.flag_control_position_enabled) { - // TODO + // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust + struct vehicle_local_position_setpoint_s loc_pos_sp; + memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); + + loc_pos_sp.x = offboard_control_sp.p1; + loc_pos_sp.y = offboard_control_sp.p2; + loc_pos_sp.yaw = offboard_control_sp.p3; + loc_pos_sp.z = offboard_control_sp.p4; + + if (_local_pos_sp_pub < 0) { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); + + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); + } } else if (_control_mode.flag_control_velocity_enabled) { // TODO diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index dc31b4c5a..a2736969f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,6 +137,7 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _local_pos_sp_pub orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; -- cgit v1.2.3 From 1fc859b1b195b587e70e4a8f6fbeb952424d0610 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Mon, 2 Jun 2014 23:11:31 -0400 Subject: Added support for postion_control offboard messages in commander. --- src/modules/commander/commander.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 90efd5115..29a5f0b4a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,6 +1809,14 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = false; + 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 = true; + control_mode.flag_control_velocity_enabled = false; + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; -- cgit v1.2.3 From 55cf19b0826911c88ca1f62f90e4d1c7e9ddf2b2 Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Tue, 3 Jun 2014 21:45:29 -0400 Subject: added support for offboard position setpoint in mc_pos_control --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 09960d106..d870b733f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -118,6 +118,7 @@ private: int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _offboard_control_pos_sp_sub; /**< offboard control position setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -132,6 +133,8 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct offboard_control_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint + struct { param_t thr_min; @@ -277,6 +280,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + memset(&_offboard_control_pos_sp, 0, sizeof(_offboard_control_pos_sp)); memset(&_ref_pos, 0, sizeof(_ref_pos)); @@ -528,6 +532,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); parameters_update(true); @@ -664,6 +669,23 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } + } else if (_control_mode.flag_control_offboard_enabled) { + /* Offboard control */ + /* Team elikos doesn't really know what it is doing */ + bool updated; + orb_check(_offboard_control_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_control_pos_sp_sub, + &_offboard_control_pos_sp); + } + + /* Hopefully no one dies from this */ + pos(0) = _offboard_control_pos_sp.p1; + pos(1) = _offboard_control_pos_sp.p2; + pos(2) = _offboard_control_pos_sp.p4; + _att_sp.yaw_body = _offboard_control_pos_sp.p3; + } else { /* AUTO */ bool updated; -- cgit v1.2.3 From ce1251fcc8119b2c4e8c9a3b5e7f0e0104fc975f Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Tue, 3 Jun 2014 22:30:14 -0400 Subject: Quick fix --- src/modules/mavlink/mavlink_receiver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a2736969f..db9e06461 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,7 +137,7 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub + orb_advert_t _local_pos_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; -- cgit v1.2.3 From e12d3af503c5ebc105e5b3c3ef8e51bd75959329 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 3 Jun 2014 23:38:18 -0400 Subject: Added support for offboard velocity control & fixed position control flags --- src/modules/commander/commander.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 29a5f0b4a..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,12 +1809,12 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + 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 = true; + control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1825,13 +1825,21 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; break; default: control_mode.flag_control_rates_enabled = false; -- cgit v1.2.3 From f1b6a3f44f3c1ade07de2a1a996b055eccd5155c Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 3 Jun 2014 23:55:21 -0400 Subject: Fixed offboard_control_pos_sp to be passed to _pos_sp vector. Added checks for position and altitude control modes. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d870b733f..b2693345c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -681,9 +681,16 @@ MulticopterPositionControl::task_main() } /* Hopefully no one dies from this */ - pos(0) = _offboard_control_pos_sp.p1; - pos(1) = _offboard_control_pos_sp.p2; - pos(2) = _offboard_control_pos_sp.p4; + /* Make sure position control is selected i.e. not only velocity control */ + if (_control_mode.flag_control_position_enabled) { + _pos_sp(0) = _offboard_control_pos_sp.p1; + _pos_sp(1) = _offboard_control_pos_sp.p2; + } + + if (_control_mode.flag_control_altitude_enabled) { + _pos_sp(2) = _offboard_control_pos_sp.p4; + } + _att_sp.yaw_body = _offboard_control_pos_sp.p3; } else { -- cgit v1.2.3 From abbf57dac655bb0832052efea9841b7e41525799 Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Wed, 4 Jun 2014 00:04:31 -0400 Subject: had the wrong variable and wrong setpoint type --- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 78c50c422..2157ae00b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,6 +379,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + ml_armed = true; break; default: break; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d870b733f..4d0c3216a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -133,7 +133,7 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - struct offboard_control_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint + struct vehicle_local_position_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint */ struct { @@ -532,7 +532,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); parameters_update(true); @@ -676,15 +676,15 @@ MulticopterPositionControl::task_main() orb_check(_offboard_control_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), _offboard_control_pos_sp_sub, + orb_copy(ORB_ID(vehicle_local_position_setpoint), _offboard_control_pos_sp_sub, &_offboard_control_pos_sp); } /* Hopefully no one dies from this */ - pos(0) = _offboard_control_pos_sp.p1; - pos(1) = _offboard_control_pos_sp.p2; - pos(2) = _offboard_control_pos_sp.p4; - _att_sp.yaw_body = _offboard_control_pos_sp.p3; + _pos_sp(0) = _offboard_control_pos_sp.x; + _pos_sp(1) = _offboard_control_pos_sp.y; + _pos_sp(2) = _offboard_control_pos_sp.z; + _att_sp.yaw_body = _offboard_control_pos_sp.yaw; } else { /* AUTO */ -- cgit v1.2.3 From af56879fdd94c1b712657e071324b3a6f147caa7 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Wed, 4 Jun 2014 02:32:44 -0400 Subject: Removed duplicate _pos_sp assignation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ------ 1 file changed, 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b73becf3c..87a8385d3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -680,12 +680,6 @@ MulticopterPositionControl::task_main() &_offboard_control_pos_sp); } - /* Hopefully no one dies from this */ - _pos_sp(0) = _offboard_control_pos_sp.x; - _pos_sp(1) = _offboard_control_pos_sp.y; - _pos_sp(2) = _offboard_control_pos_sp.z; - _att_sp.yaw_body = _offboard_control_pos_sp.yaw; - /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { _pos_sp(0) = _offboard_control_pos_sp.x; -- cgit v1.2.3 From 2a79a9a4e4ebae2e85dab64ae98ff2a17e421ea2 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Thu, 5 Jun 2014 04:03:40 -0400 Subject: Close fds when not needed File descriptors get closed when not needed by offboard mode to allow position and attitude controllers to advertise and publish. --- src/modules/mavlink/mavlink_receiver.cpp | 39 ++++++++++++++++++++++ src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 ++++- 3 files changed, 53 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2157ae00b..3b3423a4b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -422,6 +422,17 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.yaw = offboard_control_sp.p3; loc_pos_sp.z = offboard_control_sp.p4; + /* Close fds to allow position controller to use attitude controller */ + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -444,6 +455,12 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message att_sp.timestamp = hrt_absolute_time(); + /* Close fd to allow attitude controller to publish its own rates sp*/ + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -993,6 +1010,28 @@ MavlinkReceiver::receive_thread(void *arg) } } } + /* Close unused fds when not in offboard mode anymore */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + if (!_control_mode.flag_control_offboard_enabled) { + if (_local_pos_sp_pub > 0) { + close(_local_pos_sp_pub); + _local_pos_sp_pub = -1; + } + + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + } + } } return NULL; 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 20e016da3..26c0c386f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -584,9 +584,15 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - } else { + } else if (!_v_control_mode.flag_control_offboard_enabled) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } + + /* Close fd to let offboard att sp be advertised in mavlink receiver*/ + if (_v_control_mode.flag_control_offboard_enabled && _att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } } /* rotation matrix for current state */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 87a8385d3..4ee78516f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -734,10 +734,16 @@ MulticopterPositionControl::task_main() if (_local_pos_sp_pub > 0) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else { + } else if (!_control_mode.flag_control_offboard_enabled) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } + /* Close fd to let offboard pos sp be advertised in mavlink receiver*/ + if (_control_mode.flag_control_offboard_enabled && _local_pos_sp_pub > 0) { + close(_local_pos_sp_pub); + _local_pos_sp_pub = -1; + } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); -- cgit v1.2.3 From 9f754e0e9a2e2ad75a3fe5a15690cf542ce08e8a Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 14:05:35 +0200 Subject: U-blox driver rework, step 3 More sat info isolation Flush input after changing baudrate to allow driver restart w/o GNSS module restart --- src/drivers/gps/gps.cpp | 98 ++++++++++++++----------- src/drivers/gps/ubx.cpp | 119 ++++++++++++++----------------- src/drivers/gps/ubx.h | 10 +-- src/modules/uORB/topics/satellite_info.h | 17 +++-- 4 files changed, 126 insertions(+), 118 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d2ffc30ba..d8e2bd797 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include @@ -106,8 +107,10 @@ private: bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output @@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), - _report_pub(-1), + _report_gps_pos_pub(-1), + _report_sat_info_pub(-1), _rate(0.0f), _fake_gps(fake_gps) { @@ -172,7 +176,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : /* we need this potentially before it could be set in task_main */ g_dev = this; - memset(&_report, 0, sizeof(_report)); + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + memset(&_report_sat_info, 0, sizeof(_report_sat_info)); _debug_enabled = true; } @@ -271,33 +276,33 @@ GPS::task_main() if (_fake_gps) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)1200e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 0.9f; - _report.epv_m = 1.8f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.p_variance_m = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph_m = 0.9f; + _report_gps_pos.epv_m = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } @@ -313,11 +318,11 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); break; case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); + _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: @@ -332,20 +337,33 @@ GPS::task_main() // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (helper_ret & 1) { + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + if (helper_ret & 2) { + if (_report_sat_info_pub > 0) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_report_sat_info); + } } } - last_rate_count++; + if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + last_rate_count++; + } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { @@ -357,7 +375,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -447,11 +465,11 @@ 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: %8.4fms ago", (int)_report.fix_type, - _report.satellites_used, (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", (double)_report.eph_m, (double)_report.epv_m); + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph_m, (double)_report_gps_pos.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); @@ -578,7 +596,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 462875b6c..dd47572b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -65,9 +65,13 @@ #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : _fd(fd), _gps_position(gps_position), + _satellite_info(satellite_info), + _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), _disable_cmd_last(0) @@ -84,14 +88,19 @@ UBX::configure(unsigned &baudrate) { _configured = false; /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200}; - int baud_i; + unsigned baud_i; - for (baud_i = 0; baud_i < 5; baud_i++) { - baudrate = baudrates_to_try[baud_i]; + for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) { + baudrate = baudrates[baud_i]; set_baudrate(_fd, baudrate); + /* flush input and wait for at least 20 ms silence */ + decode_init(); + wait_for_ack(20); + decode_init(); + /* Send a CFG-PRT message to set the UBX protocol for in and out * and leave the baudrate as it is, we just want an ACK-ACK from this */ @@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = baudrate; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate) break; } - if (baud_i >= 5) { + if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { return 1; } @@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate) return 1; } -#ifdef UBX_ENABLE_NAV_SVINFO - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + if (_enable_sat_info) { + 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; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } } -#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -244,14 +253,13 @@ int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - uint64_t time_started = hrt_absolute_time(); + uint64_t time_end = hrt_absolute_time() + timeout * 1000; - while (hrt_absolute_time() < time_started + timeout * 1000) { + while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { if (receive(timeout) > 0) { if (!_waiting_for_ack) { return 1; } - } else { return -1; // timeout or error receiving, or NAK } @@ -275,7 +283,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool handled = false; + int handled = 0; while (true) { @@ -290,7 +298,7 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ if (handled) { - return 1; + return handled; } else { return -1; @@ -311,8 +319,7 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - if (handle_message() > 0) - handled = true; + handled |= handle_message(); } } } @@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_UNINIT: if (b == UBX_SYNC1) { _decode_state = UBX_DECODE_GOT_SYNC1; + //### printf("\nA"); } break; @@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_GOT_SYNC1: if (b == UBX_SYNC2) { _decode_state = UBX_DECODE_GOT_SYNC2; + //### printf("B"); } else { /* Second start symbol was wrong, reset state machine */ decode_init(); /* don't return error, it can be just false sync1 */ + //### printf("-"); } break; @@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b) add_byte_to_checksum(b); _message_class = b; _decode_state = UBX_DECODE_GOT_CLASS; + //### printf("C"); break; case UBX_DECODE_GOT_CLASS: add_byte_to_checksum(b); _message_id = b; _decode_state = UBX_DECODE_GOT_MESSAGEID; + //### printf("D"); break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; + //### printf("E"); break; case UBX_DECODE_GOT_LENGTH1: add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; + //### printf("F"); break; case UBX_DECODE_GOT_LENGTH2: @@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b) if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes /* compare checksum */ if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + //### printf("O\n"); return 1; // message received successfully } else { @@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b) } else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; + //### printf("."); } else { warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); @@ -489,65 +505,37 @@ UBX::handle_message() break; } -#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); + if (!_enable_sat_info) { // if satellite info processing not enabled: + break; // ignore and disable NAV-SVINFO message + } 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]); + for (_satellite_info->count = 0; + _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); + _satellite_info->count++) { + /* set pointer to satellite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* 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; + _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; + _satellite_info->snr[_satellite_info->count] = packet_part2->cno; + _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); + _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _satellite_info->svid[_satellite_info->count] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ - /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones - /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ + _satellite_info->timestamp = hrt_absolute_time(); - 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; + ret = 2; break; } -#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -650,7 +638,6 @@ UBX::handle_message() warnx("ubx: not acknowledged"); /* configuration obviously not successful */ _waiting_for_ack = false; - ret = -1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index b844c315e..90aeffedc 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,9 +48,7 @@ #include "gps_helper.h" -/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#undef UBX_ENABLE_NAV_SVINFO +#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -378,7 +376,7 @@ typedef enum { /* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ /* TODO: make this a command line option, allocate buffer dynamically */ #define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#ifdef UBX_ENABLE_NAV_SVINFO +#if (UBX_ENABLE_NAV_SVINFO != 0) #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #else #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) @@ -387,7 +385,7 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); ~UBX(); int receive(unsigned timeout); int configure(unsigned &baudrate); @@ -434,6 +432,8 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; bool _configured; bool _waiting_for_ack; uint8_t _message_class_needed; diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h index 5f799eda4..37c2faa96 100644 --- a/src/modules/uORB/topics/satellite_info.h +++ b/src/modules/uORB/topics/satellite_info.h @@ -53,14 +53,17 @@ /** * GNSS Satellite Info. */ + +#define SAT_INFO_MAX_SATELLITES 20 + struct satellite_info_s { - uint64_t timestamp; /**< Timestamp of satellite information */ - uint8_t count; /**< Number of satellites in satellite_...[] arrays */ - uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + uint64_t timestamp; /**< Timestamp of satellite info */ + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ }; /** -- cgit v1.2.3 From a6cf04a6ff623b7d39a97c70f837198b6c064f5b Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 8 Jun 2014 14:08:22 -0400 Subject: Create system state entry in dataman - ATTN Anton Create persistent system state id for data manager to store system state that will persist across resets. --- ROMFS/px4fmu_common/init.d/rcS | 10 +++--- src/modules/dataman/dataman.c | 50 ++++++++++++++++++++++++++--- src/modules/dataman/dataman.h | 26 ++++++++++++++- src/modules/mavlink/mavlink_main.cpp | 54 ++++++++++++++++++++++++++++++-- src/modules/navigator/navigator_main.cpp | 9 ++++++ 5 files changed, 136 insertions(+), 13 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d06f897a..13c08f405 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -402,6 +402,11 @@ then fi fi + # + # Start the datamanager + # + dataman start + # # MAVLink # @@ -544,11 +549,6 @@ then fi fi - # - # Start the datamanager - # - dataman start - # # Start the navigator # diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..96dc98f2a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -61,6 +61,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); __EXPORT int dm_clear(dm_item_t item); +__EXPORT void dm_lock(dm_item_t item); +__EXPORT void dm_unlock(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); /** Types of function calls supported by the worker task */ @@ -113,12 +115,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_SYSTEM_STATE_MAX }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; +/* Item type lock mutexes */ +static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static sem_t g_sys_state_mutex; + /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; static const char *k_data_manager_device_path = "/fs/microsd/dataman"; @@ -138,22 +145,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, dataman task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ + q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void @@ -567,6 +574,32 @@ dm_clear(dm_item_t item) return enqueue_work_item_and_wait_for_result(work); } +__EXPORT void +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_wait(g_item_locks[item]); + } +} + +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_post(g_item_locks[item]); + } +} + /* Tell the data manager about the type of the last reset */ __EXPORT int dm_restart(dm_reset_reason reason) @@ -607,6 +640,12 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; + /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + g_item_locks[i] = NULL; + g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -742,6 +781,7 @@ task_main(int argc, char *argv[]) destroy_q(&g_work_q); destroy_q(&g_free_q); sem_destroy(&g_work_queued_sema); + sem_destroy(&g_sys_state_mutex); return 0; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 1dfb26f73..f2b6cd4d3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,6 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -62,7 +63,8 @@ extern "C" { DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_SYSTEM_STATE_MAX = 1 }; /** Data persistence levels */ @@ -101,6 +103,18 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); + /** Lock all items of this type */ + __EXPORT void + dm_lock( + dm_item_t item /* The item type to clear */ + ); + + /** Unlock all items of this type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to clear */ + ); + /** Erase all items of this type */ __EXPORT int dm_clear( @@ -113,6 +127,16 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); + /* NOTE: The following structure defines the persistent system state data stored in the single + entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that + needs to survive restarts. This definition is application specific so it doesn't really belong + in this header, but till I find it a better home here it is */ + + typedef struct { + char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ + } persistent_system_state_t; + #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e300be074..cb1b762a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -952,6 +952,8 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + persistent_system_state_t sys_state; + state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; state->current_state = MAVLINK_WPM_STATE_IDLE; @@ -962,6 +964,10 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + state->current_dataman_id = sys_state.current_offboard_waypoint_id; + } } /* @@ -1428,12 +1434,38 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.dataman_id = 0; } - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_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; } + // offboard mission data saved correctly, now update the persistent system state + { + persistent_system_state_t state; + bool dm_result; + // Since we are doing a read-modify-write we must lock the item type + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so for now there + // is nothing to do here. + } + state.current_offboard_waypoint_id = mission.dataman_id; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + 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; + } + } + // if (wp.current) { // warnx("current is: %d", wp.seq); @@ -1485,7 +1517,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) publish_mission(); if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + persistent_system_state_t state; + bool dm_result; + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so we can deal with it here. + } + state.current_offboard_waypoint_id = -1; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 06e0c5904..04ea7da0e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,6 +603,15 @@ Navigator::task_main() warnx("Could not clear geofence"); } +#if 0 // *** UNTESTED... Anton, this is for you + /* Get the last offboard mission id */ + persistent_system_state_t sys_state; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + } +#endif + /* * do subscriptions */ -- cgit v1.2.3 From 0a86fd0d9f3f3ba1a007ff54e1d498e7735d4aa5 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Sun, 8 Jun 2014 15:21:02 -0400 Subject: Changed struct name used for local_pos_sp --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4ee78516f..6a105762b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -118,7 +118,7 @@ private: int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _offboard_control_pos_sp_sub; /**< offboard control position setpoint */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -133,7 +133,6 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - struct vehicle_local_position_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint */ struct { @@ -280,7 +279,6 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); - memset(&_offboard_control_pos_sp, 0, sizeof(_offboard_control_pos_sp)); memset(&_ref_pos, 0, sizeof(_ref_pos)); @@ -532,7 +530,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); parameters_update(true); @@ -671,26 +669,25 @@ MulticopterPositionControl::task_main() } else if (_control_mode.flag_control_offboard_enabled) { /* Offboard control */ - /* Team elikos doesn't really know what it is doing */ bool updated; - orb_check(_offboard_control_pos_sp_sub, &updated); + orb_check(_local_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _offboard_control_pos_sp_sub, - &_offboard_control_pos_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, + &_local_pos_sp); } /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _offboard_control_pos_sp.x; - _pos_sp(1) = _offboard_control_pos_sp.y; + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _offboard_control_pos_sp.z; + _pos_sp(2) = _local_pos_sp.z; } - _att_sp.yaw_body = _offboard_control_pos_sp.yaw; + _att_sp.yaw_body = _local_pos_sp.yaw; } else { /* AUTO */ -- cgit v1.2.3 From 4ad435b483510158ea8a5b303cd680f9e982df84 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:09:03 +0200 Subject: dataman: allow writing empty items with nullptr pointer to data --- src/modules/dataman/dataman.c | 4 +++- src/modules/dataman/dataman.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 96dc98f2a..14fee7c9a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -324,7 +324,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index f2b6cd4d3..b25a2a5ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -133,7 +133,7 @@ extern "C" { in this header, but till I find it a better home here it is */ typedef struct { - char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + char offboard_waypoint_id; /* the index of the active offboard waypoint data */ /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ } persistent_system_state_t; -- cgit v1.2.3 From cad0877f67c393b698b8fc4f242944c9e1ba1bc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:10:24 +0200 Subject: mavlink: waypoint manager fixes, mission saving on reboot --- src/modules/mavlink/mavlink_main.cpp | 432 +++++++++++++++---------------- src/modules/mavlink/mavlink_main.h | 13 +- src/modules/navigator/navigator_main.cpp | 6 +- src/modules/uORB/topics/mission.h | 4 +- 4 files changed, 227 insertions(+), 228 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb1b762a7..4e0597e80 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -954,20 +954,53 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { persistent_system_state_t sys_state; + /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->dataman_id = 0; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; + state->current_dataman_id = 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; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - state->current_dataman_id = sys_state.current_offboard_waypoint_id; + + int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + if (sys_state_size == sizeof(sys_state)) { + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { + state->dataman_id = sys_state.offboard_waypoint_id; + + if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } + + /* count waypoints in current waypoints storage */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + int seq = 0; + while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + seq++; + } + + if (_verbose) { warnx("WPM init: found %d items", seq); } + + state->size = seq; + + } else { + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + } + + } else { + if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } + + /* init mission topic */ + mission.count = state->size; + mission.dataman_id = state->dataman_id; + mission.current_index = -1; // TODO store current index in dataman? + + publish_mission(); } /* @@ -985,7 +1018,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } /* @@ -1009,13 +1042,12 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - if (_verbose) { warnx("ERROR: index out of bounds"); } + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); } } @@ -1031,25 +1063,15 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - dm_item_t dm_current; - - if (_wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { + if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1062,13 +1084,13 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } } 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); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } } } @@ -1084,12 +1106,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } + if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } } } @@ -1110,7 +1132,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -1120,7 +1142,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; @@ -1143,23 +1165,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - _wpm->timestamp_lastaction = now; + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { + _wpm->timestamp_lastaction = now; - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (_wpm->current_wp_id == _wpm->size - 1) { + if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; + } } - } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } } - break; } @@ -1172,6 +1196,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } mission.current_index = wpc.seq; publish_mission(); @@ -1180,16 +1205,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // mavlink_wpm_send_waypoint_current(wpc.seq); } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); } } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); } } @@ -1205,23 +1229,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; _wpm->current_partner_compid = msg->compid; } else { - if (_verbose) { warnx("No waypoints send"); } + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } } _wpm->current_count = _wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); } } @@ -1232,88 +1255,73 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - - break; - } + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { + _wpm->timestamp_lastaction = now; - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq >= _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + break; + } - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq == 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - - break; - } + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + break; + } - if (wpr.seq == _wpm->current_wp_id) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq == _wpm->current_wp_id) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else if (wpr.seq == _wpm->current_wp_id + 1) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else if (wpr.seq == _wpm->current_wp_id + 1) { + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + break; + } } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); break; } - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - break; - } + if (wpr.seq < _wpm->size) { + mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - 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); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } } - break; } @@ -1327,48 +1335,51 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (_wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + /* looks like our WAYPOINT_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + break; } } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + break; } + + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + _wpm->current_wp_id = 0; + _wpm->current_partner_sysid = msg->sysid; + _wpm->current_partner_compid = msg->compid; + _wpm->current_count = wpc.count; + _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } break; @@ -1378,32 +1389,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_item_decode(msg, &wp); if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - /* - * ensure that we are in the correct state and that the first waypoint has id 0 + /* Ensure that we are in the correct state and that the first waypoint has id 0 * and the following waypoints have the correct ids */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } break; } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1412,86 +1419,92 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } - ssize_t len = sizeof(struct mission_item_s); + dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - dm_item_t dm_next; + if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } - if (_wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_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; } - // offboard mission data saved correctly, now update the persistent system state - { + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _wpm->current_wp_id = wp.seq + 1; + + if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } + + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + /* write terminator item */ + if (_wpm->current_count < _wpm->max_size) { + if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + break; + } + } + + /* offboard mission data saved correctly, now update the persistent system state */ persistent_system_state_t state; bool dm_result; - // Since we are doing a read-modify-write we must lock the item type + + /* since we are doing a read-modify-write we must lock the item type */ dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so for now there - // is nothing to do here. - } - state.current_offboard_waypoint_id = mission.dataman_id; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - 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; - } - } + /* first read in the current state data, there may eventually be data other than the offboard index + * and we must preserve it */ + ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + if (dm_state_size != sizeof(state)) { + warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + } -// if (wp.current) { -// warnx("current is: %d", wp.seq); -// mission.current_index = wp.seq; -// } - // XXX ignore current set - mission.current_index = -1; + /* set new dataman storage ID */ + state.offboard_waypoint_id = _wpm->current_dataman_id; - _wpm->current_wp_id = wp.seq + 1; + /* write back to dataman */ + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + dm_unlock(DM_KEY_SYSTEM_STATE); - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } + if (dm_result) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + 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"); - mission.count = _wpm->current_count; + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - publish_mission(); + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - _wpm->current_dataman_id = mission.dataman_id; - _wpm->size = _wpm->current_count; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; + mission.current_index = 0; - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + publish_mission(); + } } else { + /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1506,46 +1519,34 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - _wpm->timestamp_lastaction = now; + /* clear only active dataman storage, don't change storage id */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - _wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - persistent_system_state_t state; - bool dm_result; - dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so we can deal with it here. - } - state.current_offboard_waypoint_id = -1; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } + if (dm_clear(dm_id) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + /* update WPM state */ + _wpm->timestamp_lastaction = now; + _wpm->size = 0; + + /* update mission topic */ + mission.count = 0; + mission.current_index = -1; + + publish_mission(); } else { + if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } - } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } } @@ -1570,7 +1571,6 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) } - int Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b85..fbb25029d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -84,18 +84,19 @@ enum MAVLINK_WPM_CODES { struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; + uint16_t size; ///< Count of waypoints in active mission + uint16_t max_size; ///< Maximal count of waypoints + int dataman_id; ///< Dataman storage ID for active mission + enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state + int16_t current_wp_id; ///< Waypoint ID in current transmission + uint16_t current_count; ///< Waypoints count in current transmission uint8_t current_partner_sysid; uint8_t current_partner_compid; + int current_dataman_id; ///< Dataman storage ID for current transmission uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; uint32_t timeout; - int current_dataman_id; }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04ea7da0e..a3426e65e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,14 +603,12 @@ Navigator::task_main() warnx("Could not clear geofence"); } -#if 0 // *** UNTESTED... Anton, this is for you /* Get the last offboard mission id */ persistent_system_state_t sys_state; if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } -#endif /* * do subscriptions diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index ef4bc1def..5f3cbcb1d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -95,8 +95,8 @@ struct mission_item_s { struct mission_s { - int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the datamanager */ + int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the dataman */ int current_index; /**< default -1, start at the one changed latest */ }; -- cgit v1.2.3 From d9b5efb263b61b8dc47ee63d7b4eb0b62853877a Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Mon, 9 Jun 2014 19:09:38 -0400 Subject: Closed additional file descriptor --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6a105762b..9d9d2a7c0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1071,6 +1071,12 @@ MulticopterPositionControl::task_main() _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; + + /* Close att_sp pub to allow offboard mode or att controller to advertise */ + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From 3015f2e7af94e684c666689aa70c602f79810218 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 10 Jun 2014 14:32:37 +0200 Subject: mavlink: retry timeout moved to define --- src/modules/mavlink/mavlink_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_main.h | 8 +++++--- src/modules/mavlink/waypoints.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4e0597e80..52cae2085 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -965,7 +965,8 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timestamp_last_send_request = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { @@ -1138,7 +1139,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("Operation timeout"); @@ -1148,7 +1149,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _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) { + } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _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); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fbb25029d..e31bbbb31 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -78,8 +78,9 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 @@ -96,7 +97,8 @@ struct mavlink_wpm_storage { uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; - uint32_t timeout; + uint32_t action_timeout; + uint32_t retry_timeout; }; diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 532eff7aa..a2109f2d8 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -88,7 +88,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; - uint32_t timeout; + uint32_t action_timeout; int current_dataman_id; }; -- cgit v1.2.3 From 578680135e6813151a791dbcc3c31b1ba9c31a97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:03:58 +0200 Subject: introduce multi device support on uORB --- src/drivers/drv_accel.h | 3 ++- src/drivers/drv_baro.h | 3 ++- src/drivers/drv_gyro.h | 3 ++- src/drivers/drv_mag.h | 3 ++- src/modules/uORB/objects_common.cpp | 12 ++++++++---- 5 files changed, 16 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7d93ed938..a6abaec70 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -81,7 +81,8 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel); +ORB_DECLARE(sensor_accel0); +ORB_DECLARE(sensor_accel1); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index e2f0137ae..248b9a73d 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -63,7 +63,8 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro); +ORB_DECLARE(sensor_baro0); +ORB_DECLARE(sensor_baro1); /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 2ae8c7058..3635cbce1 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -81,7 +81,8 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro); +ORB_DECLARE(sensor_gyro0); +ORB_DECLARE(sensor_gyro1); /* * ioctl() definitions diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 06107bd3d..32e0e48b3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -79,7 +79,8 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag); +ORB_DECLARE(sensor_mag0); +ORB_DECLARE(sensor_mag1); /* * ioctl() definitions diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..8340ca28a 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,16 +46,20 @@ #include #include -ORB_DEFINE(sensor_mag, struct mag_report); +ORB_DEFINE(sensor_mag0, struct mag_report); +ORB_DEFINE(sensor_mag1, struct mag_report); #include -ORB_DEFINE(sensor_accel, struct accel_report); +ORB_DEFINE(sensor_accel0, struct accel_report); +ORB_DEFINE(sensor_accel1, struct accel_report); #include -ORB_DEFINE(sensor_gyro, struct gyro_report); +ORB_DEFINE(sensor_gyro0, struct gyro_report); +ORB_DEFINE(sensor_gyro1, struct gyro_report); #include -ORB_DEFINE(sensor_baro, struct baro_report); +ORB_DEFINE(sensor_baro0, struct baro_report); +ORB_DEFINE(sensor_baro1, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); -- cgit v1.2.3 From 96accbf96cbc0262e4bb815d722282a318e8b8c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:18 +0200 Subject: Make sensors app multi-device aware --- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..0ae93d7f0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -967,7 +967,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -993,7 +993,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1019,7 +1019,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1050,7 +1050,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1579,11 +1579,11 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); -- cgit v1.2.3 From 07fb1e089d1d15d512a8a68b03020b1ffd877ed3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:56 +0200 Subject: Make commander multi-device aware --- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index cbc2844c1..d89c67c2b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 0ead22f77..23900f386 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); struct mag_report mag; /* limit update rate to get equally spaced measurements over time (in ms) */ @@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; -- cgit v1.2.3 From 699ad1512cb55db004ab7d7143dd1ef8741fb9ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:16 +0200 Subject: EKF estimator: Add support for multi uORB sensor topics --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 0a6d3fa8f..d806b0241 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 @@ -522,7 +522,7 @@ FixedwingEstimator::task_main() /* * do subscriptions */ - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -861,7 +861,7 @@ FixedwingEstimator::task_main() orb_check(_baro_sub, &baro_updated); if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; @@ -1040,7 +1040,7 @@ FixedwingEstimator::task_main() float gps_alt = _gps.alt / 1e3f; // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_gps_offset = _baro_ref - _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); -- cgit v1.2.3 From e3e8bf25befa23edd6e2963760794abf4c8d6e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:41 +0200 Subject: FW att control: Add support for multi uORB topics --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') 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 178b590ae..c026e29a7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); } } @@ -568,7 +568,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); -- cgit v1.2.3 From 0031713004dc9dcc071e13ee388af8ad028d3978 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:10:12 +0200 Subject: mavlink: Add support for multi uORB sensor interface --- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cf..98a7cef5a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -495,10 +495,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); } } @@ -517,10 +517,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } @@ -538,10 +538,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); } } @@ -556,10 +556,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); } } @@ -835,10 +835,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } -- cgit v1.2.3 From ca6463efd816a6bb01bf0ff54260e2fef33e1d84 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 10 Jun 2014 19:24:03 -0400 Subject: Modified mavlink receiver to scale offboard position and velocity messages to centimeters --- src/modules/mavlink/mavlink_receiver.cpp | 27 ++++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3b3423a4b..de9a6e7ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -365,30 +365,47 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message case 1: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; + break; case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; + break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; ml_armed = true; + break; case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; ml_armed = true; + break; default: break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + + } else if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + + /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ + /* Converts INT16 centimeters to float meters */ + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f; + + } if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; @@ -420,7 +437,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.x = offboard_control_sp.p1; loc_pos_sp.y = offboard_control_sp.p2; loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = offboard_control_sp.p4; + loc_pos_sp.z = -offboard_control_sp.p4; /* Close fds to allow position controller to use attitude controller */ if (_att_sp_pub > 0) { -- cgit v1.2.3 From 2951962c2104a0b2a284a7c5208171b257ed9734 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 12 Jun 2014 13:11:45 +0200 Subject: dataman: rename SYSTEM_STATE section to MISSION_STATE --- src/modules/dataman/dataman.c | 6 +++--- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++--------- src/modules/navigator/navigator_main.cpp | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) (limited to 'src/modules') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 14fee7c9a..4a89c7d41 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -116,7 +116,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX, - DM_KEY_SYSTEM_STATE_MAX + DM_KEY_MISSION_STATE_MAX }; /* Table of offset for index 0 of each item type */ @@ -642,11 +642,11 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; - /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) g_item_locks[i] = NULL; - g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index b25a2a5ef..215ec4c76 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -64,7 +64,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_SYSTEM_STATE_MAX = 1 + DM_KEY_MISSION_STATE_MAX = 1 }; /** Data persistence levels */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9159f5d61..be1fc50a6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -969,7 +969,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { state->dataman_id = sys_state.offboard_waypoint_id; @@ -990,11 +990,11 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->size = seq; } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } } } else { - if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } + if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } /* init mission topic */ @@ -1467,25 +1467,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) bool dm_result; /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_SYSTEM_STATE); + dm_lock(DM_KEY_MISSION_STATE); /* first read in the current state data, there may eventually be data other than the offboard index * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); if (dm_state_size != sizeof(state)) { - warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); } /* set new dataman storage ID */ state.offboard_waypoint_id = _wpm->current_dataman_id; /* write back to dataman */ - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); + dm_unlock(DM_KEY_MISSION_STATE); if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } 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"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3426e65e..c7b39ba93 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -605,7 +605,7 @@ Navigator::task_main() /* Get the last offboard mission id */ persistent_system_state_t sys_state; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } -- cgit v1.2.3 From 91b590ef584cfc67be7555e3d7272bb94bc9b2b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 23:40:48 +0200 Subject: Move MISSION_STATE read/write from mavlink to navigator and commander --- src/modules/commander/commander.cpp | 22 ++++-- src/modules/dataman/dataman.h | 12 +--- src/modules/mavlink/mavlink_main.cpp | 119 ++++++------------------------- src/modules/navigator/mission.cpp | 33 +++++++++ src/modules/navigator/mission.h | 5 ++ src/modules/navigator/navigator_main.cpp | 7 -- 6 files changed, 80 insertions(+), 118 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..d2f8c2994 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -89,6 +90,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); + /* init mission state */ + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", + mission.dataman_id, mission.count, mission.current_index); + orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + close(mission_pub); + } else { + mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + } } mavlink_log_info(mavlink_fd, "[cmd] started"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 215ec4c76..dbace21ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -127,16 +127,6 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); - /* NOTE: The following structure defines the persistent system state data stored in the single - entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that - needs to survive restarts. This definition is application specific so it doesn't really belong - in this header, but till I find it a better home here it is */ - - typedef struct { - char offboard_waypoint_id; /* the index of the active offboard waypoint data */ - /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ - } persistent_system_state_t; - #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c6ef68ee..bc4376b84 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * 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, @@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { - persistent_system_state_t sys_state; - /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); - if (sys_state_size == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { - state->dataman_id = sys_state.offboard_waypoint_id; - - if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } - - /* count waypoints in current waypoints storage */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - int seq = 0; - while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - seq++; - } - - if (_verbose) { warnx("WPM init: found %d items", seq); } - - state->size = seq; - - } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } - } - - } else { - if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } - } - - /* init mission topic */ - mission.count = state->size; - mission.dataman_id = state->dataman_id; - mission.current_index = -1; // TODO store current index in dataman? - - publish_mission(); + } /* @@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } } } @@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } @@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + mission.current_index = -1; + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } 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"); @@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } + /* waypoint marked as current */ + if (wp.current) { + mission.current_index = wp.seq; + } + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _wpm->current_wp_id = wp.seq + 1; @@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - /* write terminator item */ - if (_wpm->current_count < _wpm->max_size) { - if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - break; - } - } + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - /* offboard mission data saved correctly, now update the persistent system state */ - persistent_system_state_t state; - bool dm_result; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; - /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_MISSION_STATE); - - /* first read in the current state data, there may eventually be data other than the offboard index - * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); - if (dm_state_size != sizeof(state)) { - warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); - } - - /* set new dataman storage ID */ - state.offboard_waypoint_id = _wpm->current_dataman_id; - - /* write back to dataman */ - dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - - dm_unlock(DM_KEY_MISSION_STATE); - - if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } - - 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"); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; - mission.current_index = 0; - - publish_mission(); - } + publish_mission(); } else { /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } - break; } @@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; mission.count = 0; mission.current_index = -1; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..d9d8353f6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -430,6 +430,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } +void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* check if state actually changed to save flash write cycles */ + if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || + mission_state.count != _offboard_mission.count || + mission_state.current_index != _current_offboard_mission_index) { + + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_index = _current_offboard_mission_index; + + /* write modifyed state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + void Mission::report_mission_item_reached() { @@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96a..98b11bade 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -141,6 +141,11 @@ private: bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Save current offboard mission state to dataman + */ + void save_offboard_mission_state(); + /** * Report that a mission item has been reached */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 888c8e7f4..a3c190c7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -227,13 +227,6 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* Get the last offboard mission id */ - persistent_system_state_t sys_state; - if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); - } - /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); -- cgit v1.2.3 From ffd9ac7e081aebb3d6329a0f6c09812d1c0c4523 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 01:31:23 +0200 Subject: mavlink: fix WPM initialization --- src/modules/mavlink/mavlink_main.cpp | 59 ++++++++++++++++++++---------------- src/modules/mavlink/mavlink_main.h | 4 ++- 2 files changed, 36 insertions(+), 27 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bc4376b84..126c4dfc6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -72,8 +72,6 @@ #include #include -#include -#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -245,7 +243,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; - mission.count = 0; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -860,10 +858,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); } } @@ -963,10 +961,20 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + /* get offboard_mission topic */ + int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { + /* error getting topic, init to safe values */ + _mission.dataman_id = 0; + _mission.count = 0; + _mission.current_index = -1; + } + close(mission_sub); + /* init WPM state */ - state->size = 0; + state->size = _mission.count; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = 0; + state->dataman_id = _mission.dataman_id; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; @@ -977,7 +985,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - + warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); } /* @@ -1035,7 +1043,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_system = sysid; wpc.target_component = compid; - wpc.count = mission.count; + wpc.count = _wpm->size; mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); @@ -1058,6 +1066,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; + wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); @@ -1175,7 +1184,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpc.seq < _wpm->size) { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - mission.current_index = wpc.seq; + _mission.current_index = wpc.seq; publish_mission(); } else { @@ -1353,7 +1362,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - mission.current_index = -1; + _mission.current_index = -1; mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } @@ -1418,7 +1427,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) /* waypoint marked as current */ if (wp.current) { - mission.current_index = wp.seq; + _mission.current_index = wp.seq; } if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } @@ -1438,8 +1447,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = _wpm->current_count; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = _wpm->current_count; publish_mission(); @@ -1471,9 +1480,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = 0; - mission.current_index = -1; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = 0; + _mission.current_index = -1; publish_mission(); @@ -1754,7 +1763,6 @@ Mavlink::pass_message(mavlink_message_t *msg) } - int Mavlink::task_main(int argc, char *argv[]) { @@ -1918,8 +1926,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_init(_wpm); int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); + memset(&_mission_result, 0, sizeof(_mission_result)); _task_running = true; @@ -2027,19 +2034,19 @@ Mavlink::task_main(int argc, char *argv[]) orb_check(mission_result_sub, &updated); if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } + if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + if (_mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); } - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } else { if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 61fd7afe2..5d7273abd 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,6 +50,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -238,7 +239,8 @@ private: MavlinkStream *_streams; orb_advert_t _mission_pub; - struct mission_s mission; + struct mission_s _mission; + struct mission_result_s _mission_result; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; -- cgit v1.2.3 From 7d05f2df7cf50dea6d2960001b5b0af7236f9e5e Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Fri, 13 Jun 2014 20:38:43 -0400 Subject: Added support for velocity setpoint in mavlink_receiver and mc_pos_control --- src/modules/mavlink/mavlink_receiver.cpp | 36 +++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 ++++++++++++++++- 3 files changed, 64 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index de9a6e7ad..8a00509a4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -103,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), + _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _vicon_position_pub(-1), @@ -450,6 +451,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message _rates_sp_pub = -1; } + if (_global_vel_sp_pub > 0) { + close(_global_vel_sp_pub); + _global_vel_sp_pub = -1; + } + if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -458,7 +464,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } } else if (_control_mode.flag_control_velocity_enabled) { - // TODO + /* velocity control */ + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); + + global_vel_sp.vx = offboard_control_sp.p1; + global_vel_sp.vy = offboard_control_sp.p2; + global_vel_sp.vz = offboard_control_sp.p3; + + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + + if (_global_vel_sp_pub < 0) { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint_s), &_global_vel_sp_pub); + + } else { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint_s), _global_vel_sp_pub, &global_vel_sp); + } } else if (_control_mode.flag_control_attitude_enabled) { /* attitude control */ @@ -1038,6 +1067,11 @@ MavlinkReceiver::receive_thread(void *arg) _local_pos_sp_pub = -1; } + if (_global_vel_sp_pub > 0) { + close(_global_vel_sp_pub); + _global_vel_sp_pub = -1; + } + if (_att_sp_pub > 0) { close(_att_sp_pub); _att_sp_pub = -1; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index db9e06461..a20cbc7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -138,6 +139,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; + orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9d9d2a7c0..53394b41c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,6 +119,7 @@ private: int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -257,6 +258,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), + _global_vel_sp_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ @@ -784,6 +786,24 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + /* Offboard velocity control mode */ + if (_control_mode.flag_control_offboard_enabled) { + bool updated; + orb_check(_global_vel_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp); + } + + if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = _global_vel_sp.vz; + } + + if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { + _vel_sp(0) = _global_vel_sp.vx; + _vel_sp(1) = _global_vel_sp.vy; + } + if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); @@ -801,10 +821,16 @@ MulticopterPositionControl::task_main() if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else { + } else if (!_control_mode.flag_control_offboard_enabled) { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } + /* Close fd to let offboard vel sp be advertised in mavlink receiver */ + if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { + close (_global_vel_sp); + _global_vel_sp_pub = -1; + } + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { -- cgit v1.2.3 From 6cf890b46b5a112ed20b3f8ed7be7b658d5c4a8c Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Sat, 14 Jun 2014 15:27:07 -0400 Subject: indentation and fix commander flags. It's impossible to control position at the same time as attitude so we have to disable some things. My logic is that all the control flags for position control should be opposite of the attitude control mode. --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3da43a792..2c36b2e6c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,8 +1834,8 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9d9d2a7c0..6721d017d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -679,12 +679,12 @@ MulticopterPositionControl::task_main() /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; + _pos_sp(2) = _local_pos_sp.z; } _att_sp.yaw_body = _local_pos_sp.yaw; -- cgit v1.2.3 From 128ec447ad43d5a25abcbdda6de01ca7ebc681cf Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Sat, 14 Jun 2014 15:50:21 -0400 Subject: Fix various compilation issue --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 +++++---- 2 files changed, 9 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8a00509a4..c95ee880c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -390,14 +390,14 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message break; } - if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - } else if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + } else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ /* Converts INT16 centimeters to float meters */ @@ -483,10 +483,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint_s), &_global_vel_sp_pub); + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint_s), _global_vel_sp_pub, &global_vel_sp); + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); } } else if (_control_mode.flag_control_attitude_enabled) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 53394b41c..86112fa21 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -117,9 +117,9 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _local_pos_sp_sub; /**< offboard local position setpoint */ - int _global_vel_sp_sub; /**< offboard global velocity setpoint */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -802,6 +802,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { _vel_sp(0) = _global_vel_sp.vx; _vel_sp(1) = _global_vel_sp.vy; + } } if (!_control_mode.flag_control_manual_enabled) { @@ -827,7 +828,7 @@ MulticopterPositionControl::task_main() /* Close fd to let offboard vel sp be advertised in mavlink receiver */ if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { - close (_global_vel_sp); + close(_global_vel_sp_pub); _global_vel_sp_pub = -1; } -- cgit v1.2.3 From 5be741607c0d8d477ff30c7639edbd3bce427e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 23:57:29 +0200 Subject: mavlink: mission manager moved to separate class and reworked --- src/modules/commander/commander.cpp | 25 +- src/modules/mavlink/mavlink_main.cpp | 766 +++------------------------- src/modules/mavlink/mavlink_main.h | 92 +--- src/modules/mavlink/mavlink_mission.cpp | 823 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_mission.h | 176 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 13 +- src/modules/mavlink/mavlink_receiver.h | 2 - src/modules/mavlink/module.mk | 1 + src/modules/mavlink/util.h | 53 -- src/modules/mavlink/waypoints.h | 111 ----- src/modules/navigator/mission.cpp | 107 ++-- src/modules/navigator/mission.h | 2 + src/modules/uORB/topics/mission.h | 6 +- src/modules/uORB/topics/mission_result.h | 8 +- 14 files changed, 1168 insertions(+), 1017 deletions(-) create mode 100644 src/modules/mavlink/mavlink_mission.cpp create mode 100644 src/modules/mavlink/mavlink_mission.h delete mode 100644 src/modules/mavlink/util.h delete mode 100644 src/modules/mavlink/waypoints.h (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d2f8c2994..379ce45fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -715,17 +715,27 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - /* init mission state */ + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; mission_s mission; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", - mission.dataman_id, mission.count, mission.current_index); - orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - close(mission_pub); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); } else { + warnx("reading mission state failed"); mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } + + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } mavlink_log_info(mavlink_fd, "[cmd] started"); @@ -1310,7 +1320,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { @@ -1327,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { + mission_result.finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { status.failsafe_state = FAILSAFE_STATE_RC_LOSS; @@ -1488,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 126c4dfc6..f8a31a4ad 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +extern mavlink_system_t mavlink_system; + static uint64_t last_write_success_times[6] = {0}; static uint64_t last_write_try_times[6] = {0}; @@ -200,9 +202,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } - - - } static void usage(void); @@ -221,6 +220,7 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), + _mission_result_sub(-1), _mission_pub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), @@ -238,12 +238,11 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - _wpm = &_wpm_s; - fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -421,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; @@ -497,7 +496,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); @@ -701,9 +699,32 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; +void +Mavlink::send_message(const mavlink_message_t *msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; -int Mavlink::mavlink_pm_queued_send() + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + mavlink_pm_message_handler(_channel, msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +int +Mavlink::mavlink_pm_queued_send() { if (_mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); @@ -780,7 +801,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); + send_message(&tx_msg); return OK; } @@ -794,7 +815,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (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"); + send_statustext("[mavlink pm] sending list"); } } break; @@ -818,7 +839,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); + send_statustext(buf); } else { /* set and send parameter */ @@ -854,677 +875,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } } -void Mavlink::publish_mission() -{ - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); - } -} - -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - 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->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; - - return OK; -} - -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; - - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; - - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - 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->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - /* get offboard_mission topic */ - int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { - /* error getting topic, init to safe values */ - _mission.dataman_id = 0; - _mission.count = 0; - _mission.current_index = -1; - } - close(mission_sub); - - /* init WPM state */ - state->size = _mission.count; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = _mission.dataman_id; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->current_dataman_id = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timestamp_last_send_request = 0; - state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - - warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ - - } else { - if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - } -} - -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = _wpm->size; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } - - } 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("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } - } -} - -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - 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("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } -} - -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - - _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 > _wpm->retry_timeout && _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); - } -} - - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - - _mission.current_index = wpc.seq; - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq == 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } - - mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); - break; - } - - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - /* looks like our WAYPOINT_REQUEST was lost, try again */ - if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - - _mission.current_index = -1; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - - /* Ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } - - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } - - 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; - } - - /* waypoint marked as current */ - if (wp.current) { - _mission.current_index = wp.seq; - } - - if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* got all new mission items successfully */ - if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = _wpm->current_count; - - publish_mission(); - - } else { - /* request next item */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - /* clear only active dataman storage, don't change storage id */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_clear(dm_id) == OK) { - if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->timestamp_lastaction = now; - _wpm->size = 0; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = 0; - _mission.current_index = -1; - - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} - -void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +int +Mavlink::send_statustext(const char *string) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return send_statustext(MAV_SEVERITY_INFO, string); } - int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; + statustext.severity = severity; int i = 0; @@ -1546,7 +908,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) return OK; } else { - return 1; + return ERROR; } } @@ -1684,7 +1046,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1751,7 +1113,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1762,6 +1124,11 @@ Mavlink::pass_message(mavlink_message_t *msg) } } +float +Mavlink::get_rate_mult() +{ + return _datarate / 1000.0f; +} int Mavlink::task_main(int argc, char *argv[]) @@ -1879,8 +1246,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1922,11 +1287,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - memset(&_mission_result, 0, sizeof(_mission_result)); + /* create mission manager */ + _mission_manager = new MavlinkMissionManager(this); + _mission_manager->set_verbose(_verbose); _task_running = true; @@ -1941,7 +1306,7 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); @@ -1976,7 +1341,6 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ @@ -2030,36 +1394,16 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - - if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - - if (_mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - } - } - if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); + send_statustext(msg.text); } } } @@ -2093,11 +1437,11 @@ Mavlink::task_main(int argc, char *argv[]) } } - - perf_end(_loop_perf); } + delete _mission_manager; + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5d7273abd..085a97d73 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -56,51 +56,7 @@ #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds -#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; ///< Count of waypoints in active mission - uint16_t max_size; ///< Maximal count of waypoints - int dataman_id; ///< Dataman storage ID for active mission - enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state - int16_t current_wp_id; ///< Waypoint ID in current transmission - uint16_t current_count; ///< Waypoints count in current transmission - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - int current_dataman_id; ///< Dataman storage ID for current transmission - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_last_send_request; - uint32_t action_timeout; - uint32_t retry_timeout; -}; +#include "mavlink_mission.h" class Mavlink @@ -143,7 +99,7 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -168,10 +124,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); static int start_helper(int argc, char *argv[]); @@ -192,6 +144,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + void send_message(const mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -209,6 +165,10 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + int send_statustext(const char *string); + int send_statustext(enum MAV_SEVERITY severity, const char *string); + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -238,12 +198,12 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; + MavlinkMissionManager *_mission_manager; + orb_advert_t _mission_pub; - struct mission_s _mission; - struct mission_result_s _mission_result; + int _mission_result_sub; MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; @@ -251,10 +211,6 @@ private: pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -336,20 +292,9 @@ private: void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); + uint8_t get_system_id(); + + uint8_t get_component_id(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -364,13 +309,13 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -378,5 +323,4 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); - }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 000000000..1eab7d761 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,823 @@ +/**************************************************************************** + * + * 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 + * 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 mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include +#include +#include +#include + +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _count(0), + _current_seq(0), + _dataman_id(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _verbose(false) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::eventloop() +{ + hrt_abstime now = hrt_absolute_time(); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq >= 0 && wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext("WP CMD OK AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + 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->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + 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->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 000000000..d8695cf83 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 + * 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 mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class Mavlink; + +class MavlinkMissionManager { +public: + MavlinkMissionManager(Mavlink *parent); + + ~MavlinkMissionManager(); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + void send_message(mavlink_message_t *msg); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void eventloop(); + + void handle_message(const mavlink_message_t *msg); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void set_verbose(bool v) { _verbose = v; } + +private: + Mavlink* _mavlink; + mavlink_channel_t _channel; + uint8_t _comp_id; + + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + int _transfer_seq; ///< Item sequence in current transmission + int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1e217ec70..834852d6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -902,16 +901,8 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..df5d037f8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,8 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..725a9e184 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h deleted file mode 100644 index 5ca9a085d..000000000 --- a/src/modules/mavlink/util.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * 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 - * 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 util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index a2109f2d8..000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-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 waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include -#include "mavlink_bridge_header.h" -#include -#include - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t action_timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d9d8353f6..f6d304c37 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,13 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -85,16 +83,25 @@ Mission::on_inactive() { _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { - update_onboard_mission(); - } + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; + + update_onboard_mission(); update_offboard_mission(); } } @@ -105,13 +112,13 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); @@ -139,9 +146,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -154,7 +161,7 @@ Mission::update_onboard_mission() } } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -163,13 +170,12 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; /* if not initialized, set it to 0 */ @@ -194,10 +200,12 @@ Mission::update_offboard_mission() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -326,10 +334,10 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { - int next_temp_mission_index = _onboard_mission.current_index + 1; + int next_temp_mission_index = _onboard_mission.current_seq + 1; /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && + if (_onboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { @@ -349,9 +357,9 @@ void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; + int next_temp_mission_index = _offboard_mission.current_seq + 1; warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && + if (_offboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { @@ -441,19 +449,31 @@ Mission::save_offboard_mission_state() /* read current state */ int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - /* check if state actually changed to save flash write cycles */ - if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || - mission_state.count != _offboard_mission.count || - mission_state.current_index != _current_offboard_mission_index) { + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ mission_state.dataman_id = _offboard_mission.dataman_id; mission_state.count = _offboard_mission.count; - mission_state.current_index = _current_offboard_mission_index; + mission_state.current_seq = _current_offboard_mission_index; - /* write modifyed state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); } } @@ -465,8 +485,8 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -474,7 +494,8 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); save_offboard_mission_state(); @@ -483,7 +504,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -500,6 +521,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 98b11bade..1310671b0 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -183,6 +183,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4bcaaaa5a..499b2e1e5 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,11 +97,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the dataman */ - int current_index; /**< default -1, start at the one changed latest */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff..beb797e62 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** -- cgit v1.2.3 From 74ae18c245514d918e6ea58f05de98b9fe4b63a5 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Sat, 14 Jun 2014 18:46:18 -0400 Subject: Fixed rates flags --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c36b2e6c..7baaf7139 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,7 +1834,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; -- cgit v1.2.3 From 61833905472465ebab615ecdd567b78eb1628d97 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Mon, 16 Jun 2014 17:47:10 -0400 Subject: Commander attitude flag fix --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7baaf7139..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1835,7 +1835,7 @@ set_control_mode() break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; -- cgit v1.2.3 From e078ef992fc21d86cbd09db89c25332273840b22 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 17 Jun 2014 13:21:20 -0400 Subject: Removed publications closing This is an attempt to correct the offboard setpoints being passed on as "NaN" values --- src/modules/mavlink/mavlink_receiver.cpp | 59 ---------------------- src/modules/mc_att_control/mc_att_control_main.cpp | 8 +-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 20 +------- 3 files changed, 3 insertions(+), 84 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c95ee880c..5528aca5e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -440,22 +440,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.yaw = offboard_control_sp.p3; loc_pos_sp.z = -offboard_control_sp.p4; - /* Close fds to allow position controller to use attitude controller */ - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - - if (_global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -472,16 +456,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message global_vel_sp.vy = offboard_control_sp.p2; global_vel_sp.vz = offboard_control_sp.p3; - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - if (_global_vel_sp_pub < 0) { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); @@ -501,12 +475,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message att_sp.timestamp = hrt_absolute_time(); - /* Close fd to allow attitude controller to publish its own rates sp*/ - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -1056,33 +1024,6 @@ MavlinkReceiver::receive_thread(void *arg) } } } - /* Close unused fds when not in offboard mode anymore */ - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - if (!_control_mode.flag_control_offboard_enabled) { - if (_local_pos_sp_pub > 0) { - close(_local_pos_sp_pub); - _local_pos_sp_pub = -1; - } - - if (_global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - } - } } return NULL; 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 26c0c386f..20e016da3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -584,15 +584,9 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - } else if (!_v_control_mode.flag_control_offboard_enabled) { + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } - - /* Close fd to let offboard att sp be advertised in mavlink receiver*/ - if (_v_control_mode.flag_control_offboard_enabled && _att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } } /* rotation matrix for current state */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 35db05499..41fb20108 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -733,15 +733,10 @@ MulticopterPositionControl::task_main() if (_local_pos_sp_pub > 0) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else if (!_control_mode.flag_control_offboard_enabled) { + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } - /* Close fd to let offboard pos sp be advertised in mavlink receiver*/ - if (_control_mode.flag_control_offboard_enabled && _local_pos_sp_pub > 0) { - close(_local_pos_sp_pub); - _local_pos_sp_pub = -1; - } if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ @@ -822,16 +817,10 @@ MulticopterPositionControl::task_main() if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else if (!_control_mode.flag_control_offboard_enabled) { + } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } - /* Close fd to let offboard vel sp be advertised in mavlink receiver */ - if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { @@ -1099,11 +1088,6 @@ MulticopterPositionControl::task_main() reset_int_z = true; reset_int_xy = true; - /* Close att_sp pub to allow offboard mode or att controller to advertise */ - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From 9d18da4433773cfa02bec1d33fdb34e4d03d1442 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Wed, 18 Jun 2014 16:45:38 -0400 Subject: Adds NaN checks and setpoint resets for offboard posctl --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 ++++++++++++++-------- 1 file changed, 20 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 41fb20108..a4c871d95 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -675,21 +675,30 @@ MulticopterPositionControl::task_main() orb_check(_local_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, - &_local_pos_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); } - /* Make sure position control is selected i.e. not only velocity control */ - if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; - } + if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) { + /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; - if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; - } + /* Make sure position control is selected i.e. not only velocity control */ + if (_control_mode.flag_control_position_enabled) { + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; + } - _att_sp.yaw_body = _local_pos_sp.yaw; + if (_control_mode.flag_control_altitude_enabled) { + _pos_sp(2) = _local_pos_sp.z; + } + + _att_sp.yaw_body = _local_pos_sp.yaw; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } } else { /* AUTO */ -- cgit v1.2.3 From da2f68a6a09395a8d02a5d39bd3e92d7d0d79911 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Jun 2014 14:25:29 +0200 Subject: mavlink: don't lock dataman when updating mission state --- src/modules/mavlink/mavlink_mission.cpp | 6 ------ 1 file changed, 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 9bb956281..b2288469c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -128,14 +128,8 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int mission.current_seq = seq; /* update mission state in dataman */ - /* lock MISSION_STATE item */ - dm_lock(DM_KEY_MISSION_STATE); - int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); - /* unlock MISSION_STATE item */ - dm_unlock(DM_KEY_MISSION_STATE); - if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; -- cgit v1.2.3 From 88adeee6695a2160f9eac130d3923129728dd008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 13:35:14 +0200 Subject: define force setpoint uorb topic --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/vehicle_force_setpoint.h | 65 ++++++++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_force_setpoint.h (limited to 'src/modules') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..d45314222 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/vehicle_force_setpoint.h" +ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h new file mode 100644 index 000000000..c7b5f558e --- /dev/null +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * 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 vehicle_local_force_setpoint.h + * @author Thomas Gubler + * Definition of force (NED) setpoint uORB topic. Typically this can be used + * by a position control app togeth with an attitude control app. + */ + +#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ +#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_force_setpoint_s { + float x; /**< in N NED */ + float y; /**< in N NED */ + float z; /**< in N NED */ +}; /**< Desired force in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_force_setpoint); + +#endif -- cgit v1.2.3 From 179bace35aaa32a23ebd7a9cb99d54eae53690f5 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 07:47:16 -0400 Subject: Resets XY velocities when we can't estimate them --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 +++++++ .../position_estimator_inav/position_estimator_inav_params.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 3 files changed, 12 insertions(+) (limited to 'src/modules') 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 f908d7a3b..5f132453b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -916,6 +916,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } + } else { + /* gradually reset xy velocity estimates */ + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); } /* detect land */ @@ -931,6 +935,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } + /* reset xy velocity estimates when landed */ + x_est[1] = 0.0f; + y_est[1] = 0.0f; } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { 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 d88419c25..b79e00274 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,6 +46,7 @@ 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_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RESET_V, 0.2f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); @@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) 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_flow = param_find("INAV_W_XY_FLOW"); + h->w_xy_reset_v = param_find("INAV_W_XY_RESET_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str 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_flow, &(p->w_xy_flow)); + param_get(h->w_xy_reset_v, &(p->w_xy_reset_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); 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 df1cfaa71..9be13637d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,6 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; + float w_xy_reset_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; + param_t w_xy_reset_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 3ddb502d2e1b07705bfaa59c89528d53d0444303 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 14:14:13 +0200 Subject: force setpoint: fix comment --- src/modules/uORB/topics/vehicle_force_setpoint.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index c7b5f558e..cd948985f 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,10 +32,10 @@ ****************************************************************************/ /** - * @file vehicle_local_force_setpoint.h + * @file vehicle_force_setpoint.h * @author Thomas Gubler * Definition of force (NED) setpoint uORB topic. Typically this can be used - * by a position control app togeth with an attitude control app. + * by a position control app together with an attitude control app. */ #ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ -- cgit v1.2.3 From be33b4b6a56402135e5cfb8b38e0cc0ae8b7d673 Mon Sep 17 00:00:00 2001 From: holger Date: Tue, 24 Jun 2014 19:28:39 +0200 Subject: UAVCAN: append to EXTRADEFINES to those given by make cmd line --- src/modules/uavcan/module.mk | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index ce9f981a8..5ac7019e3 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -52,7 +52,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 @@ -60,8 +60,7 @@ EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=2 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler -- cgit v1.2.3 From 211c721b48c5856ace7f09e88706d799299ca6bb Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:42:25 -0400 Subject: Shorten the reset param name to INAV_W_XY_RES_V --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- .../position_estimator_inav/position_estimator_inav_params.c | 6 +++--- .../position_estimator_inav/position_estimator_inav_params.h | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules') 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 5f132453b..eca406acf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -918,8 +918,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } else { /* gradually reset xy velocity estimates */ - inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v); - inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* detect land */ 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 b79e00274..47c1a4c6c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,7 +46,7 @@ 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_FLOW, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_RESET_V, 0.2f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.2f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); @@ -66,7 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) 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_flow = param_find("INAV_W_XY_FLOW"); - h->w_xy_reset_v = param_find("INAV_W_XY_RESET_V"); + h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -89,7 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str 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_flow, &(p->w_xy_flow)); - param_get(h->w_xy_reset_v, &(p->w_xy_reset_v)); + param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); 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 9be13637d..423f0d879 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,7 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; - float w_xy_reset_v; + float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -67,7 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; - param_t w_xy_reset_v; + param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 7a45888e78b4af0c04014c20e579797b2040edf8 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:56:36 -0400 Subject: Adjusted default reset param value to reset quicker --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') 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 47c1a4c6c..0581f8236 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,7 +46,7 @@ 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_FLOW, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.2f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); -- cgit v1.2.3 From 5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:24:37 +0200 Subject: commander: detect if offboard control is lost and missing offboard states --- src/modules/commander/commander.cpp | 14 ++++++++++++++ src/modules/commander/state_machine_helper.cpp | 19 +++++++++++++++++++ 2 files changed, 33 insertions(+) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fab80e325..0c9ce4418 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -661,6 +661,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -932,6 +933,19 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + orb_check(telemetry_sub, &updated); if (updated) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 81b489ce3..4339a7a76 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -541,6 +541,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_OFFBOARD; + } default: break; } -- cgit v1.2.3 From c99683752ccb62c9654b390c8761337f291c58e9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:25:11 +0200 Subject: navigator: added missing offboard state --- src/modules/navigator/navigator_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d762b8a9d..8ab50f0e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -363,6 +363,7 @@ Navigator::task_main() break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: default: _navigation_mode = nullptr; _is_in_loiter = false; -- cgit v1.2.3 From f5699e6490a136f4344bfab10b72a9cdd4860569 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:27:11 +0200 Subject: mavlink: always copy offboard setpoints, not just when control mode changed, also bugfixes in orb calls --- src/modules/mavlink/mavlink_receiver.cpp | 123 +++++++++++++++---------------- 1 file changed, 61 insertions(+), 62 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 80404a46a..aaae0933e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -407,11 +407,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ /* Converts INT16 centimeters to float meters */ - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f; - + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f; } if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { @@ -434,83 +433,83 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message orb_check(_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } - if (_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_position_enabled) { - // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust - struct vehicle_local_position_setpoint_s loc_pos_sp; - memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - - loc_pos_sp.x = offboard_control_sp.p1; - loc_pos_sp.y = offboard_control_sp.p2; - loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = -offboard_control_sp.p4; + if (_control_mode.flag_control_offboard_enabled) { + if (_control_mode.flag_control_position_enabled) { + // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust + struct vehicle_local_position_setpoint_s loc_pos_sp; + memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - if (_local_pos_sp_pub < 0) { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); + loc_pos_sp.x = offboard_control_sp.p1; + loc_pos_sp.y = offboard_control_sp.p2; + loc_pos_sp.yaw = offboard_control_sp.p3; + loc_pos_sp.z = -offboard_control_sp.p4; - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); - } + if (_local_pos_sp_pub < 0) { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp); - } else if (_control_mode.flag_control_velocity_enabled) { - /* velocity control */ - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); + } - global_vel_sp.vx = offboard_control_sp.p1; - global_vel_sp.vy = offboard_control_sp.p2; - global_vel_sp.vz = offboard_control_sp.p3; + } else if (_control_mode.flag_control_velocity_enabled) { + /* velocity control */ + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); - if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); + global_vel_sp.vx = offboard_control_sp.p1; + global_vel_sp.vy = offboard_control_sp.p2; + global_vel_sp.vz = offboard_control_sp.p3; - } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); - } + if (_global_vel_sp_pub < 0) { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - } else if (_control_mode.flag_control_attitude_enabled) { - /* attitude control */ - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); + } else { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); + } - att_sp.roll_body = offboard_control_sp.p1; - att_sp.pitch_body = offboard_control_sp.p2; - att_sp.yaw_body = offboard_control_sp.p3; - att_sp.thrust = offboard_control_sp.p4; + } else if (_control_mode.flag_control_attitude_enabled) { + /* attitude control */ + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); - att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + att_sp.timestamp = hrt_absolute_time(); - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } else if (_control_mode.flag_control_rates_enabled) { - /* rates control */ - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } - rates_sp.roll = offboard_control_sp.p1; - rates_sp.pitch = offboard_control_sp.p2; - rates_sp.yaw = offboard_control_sp.p3; - rates_sp.thrust = offboard_control_sp.p4; + } else if (_control_mode.flag_control_rates_enabled) { + /* rates control */ + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; - if (_rates_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } + if (_rates_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } else { - /* direct control */ - // TODO + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); } + + } else { + /* direct control */ + // TODO } } } -- cgit v1.2.3 From 993e4d41d8165e27f03e51af5b809da6fd3a7509 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:27:32 +0200 Subject: sensors: missing param for offboard switch --- src/modules/sensors/sensor_params.c | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) (limited to 'src/modules') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 65e2376ce..dad81d8e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -790,3 +790,20 @@ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); + + +/** + * Threshold for selecting offboard mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel Date: Sat, 21 Jun 2014 12:28:08 +0200 Subject: mc_pos_control: added missing subscribe --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a4c871d95..e020b2224 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -134,7 +134,7 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - + struct { param_t thr_min; @@ -533,6 +533,8 @@ MulticopterPositionControl::task_main() _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + parameters_update(true); @@ -699,7 +701,7 @@ MulticopterPositionControl::task_main() reset_pos_sp(); reset_alt_sp(); } - + } else { /* AUTO */ bool updated; -- cgit v1.2.3 From 7992a063c9f9b2cf7fbb49dcd5d19c60f15d0bfc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:34:34 +0200 Subject: navigator: added offboard mode --- src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 7 ++ src/modules/navigator/offboard.cpp | 124 +++++++++++++++++++++++++++++++ src/modules/navigator/offboard.h | 82 ++++++++++++++++++++ 5 files changed, 221 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/offboard.cpp create mode 100644 src/modules/navigator/offboard.h (limited to 'src/modules') diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e109030..637eaae59 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,6 +46,7 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ + offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dadd15527..e6ff213bd 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,13 +56,14 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" +#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 3 +#define NAVIGATOR_MODE_ARRAY_SIZE 4 class Navigator : public control::SuperBlock { @@ -108,10 +109,13 @@ public: * Getters */ struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } + int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -129,6 +133,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -158,6 +163,7 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8ab50f0e1..8ba835a87 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -100,6 +101,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), + _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -121,6 +123,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _offboard(this, "OFF"), _update_triplet(false), _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") @@ -129,6 +132,7 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; + _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -241,6 +245,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -364,6 +369,8 @@ Navigator::task_main() case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: + _navigation_mode = &_offboard; + break; default: _navigation_mode = nullptr; _is_in_loiter = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp new file mode 100644 index 000000000..b0cbb1aa1 --- /dev/null +++ b/src/modules/navigator/offboard.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 offboard.cpp + * + * Helper class for offboard commands + * + * @author Julian Oes + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "navigator.h" +#include "offboard.h" + +Offboard::Offboard(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), + _offboard_control_sp({0}) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +Offboard::~Offboard() +{ +} + +bool +Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated; + orb_check(_navigator->get_offboard_control_sp_sub(), &updated); + if (updated) { + update_offboard_control_setpoint(); + } + + bool changed = false; + + /* copy offboard setpoints to the corresponding topics */ + if (_navigator->get_control_mode()->flag_control_position_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; + + changed = true; + + } else if (_navigator->get_control_mode()->flag_control_velocity_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3*10.0f; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; + + changed = true; + } + + return changed; +} + +void +Offboard::on_inactive() +{ +} + +void +Offboard::update_offboard_control_setpoint() +{ + orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); + +} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h new file mode 100644 index 000000000..4d415dddb --- /dev/null +++ b/src/modules/navigator/offboard.h @@ -0,0 +1,82 @@ +/*************************************************************************** + * + * 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 offboard.h + * + * Helper class for offboard commands + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_OFFBOARD_H +#define NAVIGATOR_OFFBOARD_H + +#include +#include + +#include +#include + +#include "navigator_mode.h" + +class Navigator; + +class Offboard : public NavigatorMode +{ +public: + /** + * Constructor + */ + Offboard(Navigator *navigator, const char *name); + + /** + * Destructor + */ + ~Offboard(); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called while the mode is active + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); +private: + void update_offboard_control_setpoint(); + + struct offboard_control_setpoint_s _offboard_control_sp; +}; + +#endif -- cgit v1.2.3 From a122e88893f4b983a6e8e2e2838f9a16f055c8d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:35:02 +0200 Subject: commander: hack to get velocity offboard control working --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c9ce4418..e4d132c40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1776,9 +1776,9 @@ set_control_mode() case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; + control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: -- cgit v1.2.3 From 4c4d14347efe9d12712399a1a5eb0140fdd113fd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:43:29 +0200 Subject: mavlink: only save a offboard_control, don't publish to setpoint topics (let the navigator do this) --- src/modules/mavlink/mavlink_receiver.cpp | 154 +++---------------------------- 1 file changed, 11 insertions(+), 143 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aaae0933e..c4955f1b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -355,70 +355,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) void MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - if (mavlink_system.sysid < 4) { + /* Only accept system IDs from 1 to 4 + * TODO: check for own system ID */ + if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; - bool ml_armed = false; + /* Convert values * 1000 back */ + offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - switch (quad_motors_setpoint.mode) { - case 0: - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - ml_armed = true; - - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - ml_armed = true; - - break; - default: - break; - } - - if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - } else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - - /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ - /* Converts INT16 centimeters to float meters */ - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f; - } - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; + offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -428,90 +380,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } - - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - - if (_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_position_enabled) { - // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust - struct vehicle_local_position_setpoint_s loc_pos_sp; - memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - - loc_pos_sp.x = offboard_control_sp.p1; - loc_pos_sp.y = offboard_control_sp.p2; - loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = -offboard_control_sp.p4; - - if (_local_pos_sp_pub < 0) { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); - } - - } else if (_control_mode.flag_control_velocity_enabled) { - /* velocity control */ - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); - - global_vel_sp.vx = offboard_control_sp.p1; - global_vel_sp.vy = offboard_control_sp.p2; - global_vel_sp.vz = offboard_control_sp.p3; - - if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - - } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); - } - - } else if (_control_mode.flag_control_attitude_enabled) { - /* attitude control */ - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - att_sp.roll_body = offboard_control_sp.p1; - att_sp.pitch_body = offboard_control_sp.p2; - att_sp.yaw_body = offboard_control_sp.p3; - att_sp.thrust = offboard_control_sp.p4; - - att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } - - } else if (_control_mode.flag_control_rates_enabled) { - /* rates control */ - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - - rates_sp.roll = offboard_control_sp.p1; - rates_sp.pitch = offboard_control_sp.p2; - rates_sp.yaw = offboard_control_sp.p3; - rates_sp.thrust = offboard_control_sp.p4; - - rates_sp.timestamp = hrt_absolute_time(); - - if (_rates_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } - - } else { - /* direct control */ - // TODO - } - } } } -- cgit v1.2.3 From df9381636a2fa96e1bc02b4244273ec6b7f11aa9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:44:05 +0200 Subject: offboard_control_setpoint: armed does not make sense there --- src/modules/uORB/topics/offboard_control_setpoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 68d3e494b..d7b131e3c 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -69,7 +69,7 @@ struct offboard_control_setpoint_s { uint64_t timestamp; enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ float p2; /**< elevator / pitch / pitch rate */ float p3; /**< rudder / yaw rate / yaw */ -- cgit v1.2.3 From 0b172d662a8926dbe1c8a0c59766f7c63bb8f4fe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:44:38 +0200 Subject: position_setpoint_triplet: add local position and velocity setpoints --- src/modules/uORB/topics/position_setpoint_triplet.h | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ce42035ba..673c0e491 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,16 +60,26 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ }; struct position_setpoint_s { bool valid; /**< true if setpoint is valid */ enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + float x; /**< local position setpoint in m in NED */ + float y; /**< local position setpoint in m in NED */ + float z; /**< local position setpoint in m in NED */ + bool position_valid; /**< true if local position setpoint valid */ + float vx; /**< local velocity setpoint in m in NED */ + float vy; /**< local velocity setpoint in m in NED */ + float vz; /**< local velocity setpoint in m in NED */ + bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ -- cgit v1.2.3 From ee872d91b05bf47d44dae4f5cc07be1631db91c1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:45:48 +0200 Subject: mc_pos_control: read velocity setpoints form offboard control and do position offset control --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 88 +++++++++++++++------- 1 file changed, 59 insertions(+), 29 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e020b2224..6541788f6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -556,6 +556,9 @@ MulticopterPositionControl::task_main() math::Vector<3> sp_move_rate; sp_move_rate.zero(); + + float yaw_sp_move_rate; + math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -674,28 +677,73 @@ MulticopterPositionControl::task_main() } else if (_control_mode.flag_control_offboard_enabled) { /* Offboard control */ bool updated; - orb_check(_local_pos_sp_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } - if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) { - /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; + if (_pos_sp_triplet.current.valid) { + + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = _pos_sp_triplet.current.vx; + sp_move_rate(1) = _pos_sp_triplet.current.vy; + yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; + _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); - /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } - _att_sp.yaw_body = _local_pos_sp.yaw; + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } } else { reset_pos_sp(); @@ -792,24 +840,6 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } - /* Offboard velocity control mode */ - if (_control_mode.flag_control_offboard_enabled) { - bool updated; - orb_check(_global_vel_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp); - } - - if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) { - _vel_sp(2) = _global_vel_sp.vz; - } - - if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { - _vel_sp(0) = _global_vel_sp.vx; - _vel_sp(1) = _global_vel_sp.vy; - } - } if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ -- cgit v1.2.3 From 93887f9f28d978290e6698b7f2b7e1e9b60bec13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 14:04:50 +0200 Subject: navigator: remove hacked scale factor for offboard control --- src/modules/navigator/offboard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index b0cbb1aa1..ef7d11a03 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -98,7 +98,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* velocity control */ pos_sp_triplet->current.vx = _offboard_control_sp.p2; pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3*10.0f; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; pos_sp_triplet->current.vz = _offboard_control_sp.p4; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; -- cgit v1.2.3 From ee6e0000980f2127913dfec73b1981b559f85877 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 14:08:13 +0200 Subject: mavlink: fix comment in swarm message receiver --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4955f1b3..c96bd9273 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -358,8 +358,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - /* Only accept system IDs from 1 to 4 - * TODO: check for own system ID */ + /* Only accept system IDs from 1 to 4 */ if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); -- cgit v1.2.3 From 456e628e129b446d18246ab8ad312a15beea5996 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 28 Jun 2014 00:54:27 +0200 Subject: navigator: NavigatorMode and MissionBlock API cleanup --- src/modules/navigator/loiter.cpp | 18 ++++++--- src/modules/navigator/loiter.h | 14 +------ src/modules/navigator/mission.cpp | 14 +++++-- src/modules/navigator/mission.h | 14 +------ src/modules/navigator/mission_block.cpp | 65 ++++++++++++++++++-------------- src/modules/navigator/mission_block.h | 14 +++---- src/modules/navigator/navigator_main.cpp | 6 +-- src/modules/navigator/navigator_mode.cpp | 34 +++++++++++++++-- src/modules/navigator/navigator_mode.h | 12 +++++- src/modules/navigator/rtl.cpp | 62 ++++++++++++++---------------- src/modules/navigator/rtl.h | 20 ++-------- 11 files changed, 144 insertions(+), 129 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 542483fb1..5e7067b0e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -65,14 +65,22 @@ Loiter::~Loiter() { } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_inactive() { - /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(pos_sp_triplet); } void -Loiter::on_inactive() +Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; +} + +bool +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { + return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 65ff5c31e..5ce86d2a7 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -50,24 +50,14 @@ class Loiter : public MissionBlock { public: - /** - * Constructor - */ Loiter(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Loiter(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 72255103b..395669698 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -82,8 +82,6 @@ Mission::~Mission() void Mission::on_inactive() { - _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -98,6 +96,12 @@ Mission::on_inactive() } } +void +Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_mission_items(pos_sp_triplet); +} + bool Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { @@ -117,10 +121,9 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) } /* reset mission items if needed */ - if (onboard_updated || offboard_updated || _first_run) { + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; - _first_run = false; } /* lets check if we reached the current mission item */ @@ -255,6 +258,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; reset_mission_item_reached(); report_mission_finished(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6e4761946..38a4f7612 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -65,24 +65,14 @@ class Navigator; class Mission : public MissionBlock { public: - /** - * Constructor - */ Mission(Navigator *navigator, const char *name); - /** - * Destructor - */ virtual ~Mission(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c7..acf3ad569 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -45,6 +45,7 @@ #include #include +#include #include @@ -207,36 +208,44 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } } -bool +void MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { - /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { - /* use current position */ - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - - _navigator->set_can_loiter_at_sp(true); - } + if (_navigator->get_vstatus()->condition_landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + _mission_item.nav_cmd = NAV_CMD_IDLE; - if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() - || pos_sp_triplet->current.loiter_direction != 1 - || pos_sp_triplet->previous.valid - || !pos_sp_triplet->current.valid - || pos_sp_triplet->next.valid) { - /* position setpoint triplet should be updated */ - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - return true; - } + _navigator->set_can_loiter_at_sp(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - return false; + if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { + /* use current position setpoint */ + _mission_item.lat = pos_sp_triplet->current.lat; + _mission_item.lon = pos_sp_triplet->current.lon; + _mission_item.altitude = pos_sp_triplet->current.alt; + + } else { + /* use current position */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index f99002752..4f79238f4 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,6 +64,7 @@ public: */ virtual ~MissionBlock(); +protected: /** * Check if mission item has been reached * @return true if successfully reached @@ -88,19 +89,16 @@ public: void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * - * @param the position setpoint triplet to set - * @return true if setpoint has changed + * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); + // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead + void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + mission_item_s _mission_item; + bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; - - mission_item_s _mission_item; - bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 546602741..266114e38 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -371,11 +371,7 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - if (_navigation_mode == _navigation_mode_array[i]) { - _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); - } else { - _navigation_mode_array[i]->on_inactive(); - } + _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 25e767c2b..6361ea9c8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -33,9 +33,10 @@ /** * @file navigator_mode.cpp * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #include "navigator_mode.h" @@ -55,16 +56,41 @@ NavigatorMode::~NavigatorMode() { } +bool +NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { + if (active) { + if (_first_run) { + /* first run */ + _first_run = false; + on_activation(pos_sp_triplet); + return true; + + } else { + /* periodic updates when active */ + on_active(pos_sp_triplet); + } + + } else { + /* periodic updates when inactive */ + _first_run = true; + on_inactive(); + } +} + void NavigatorMode::on_inactive() { - _first_run = true; +} + +void +NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* invalidate position setpoint by default */ + pos_sp_triplet->current.valid = false; } bool NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { - pos_sp_triplet->current.valid = false; - _first_run = false; return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index cbb53d91b..5c36af1fe 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -33,9 +33,10 @@ /** * @file navigator_mode.h * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #ifndef NAVIGATOR_MODE_H @@ -65,11 +66,18 @@ public: */ virtual ~NavigatorMode(); + bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is inactive */ virtual void on_inactive(); + /** + * This function is called one time when mode become active, poos_sp_triplet must be initialized here + */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is active * @@ -80,6 +88,8 @@ public: protected: Navigator *_navigator; + +private: bool _first_run; }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043f773a4..7cf6bb1a8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -75,53 +75,49 @@ RTL::~RTL() void RTL::on_inactive() { - _first_run = true; - /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _rtl_state = RTL_STATE_NONE; } } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) { - bool updated = false; - - if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } + } - set_rtl_item(pos_sp_triplet); - updated = true; + set_rtl_item(pos_sp_triplet); +} - } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { +bool +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); - updated = true; + return true; } - return updated; + return false; } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b4b729e89..9c8b3fdfc 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -57,29 +57,15 @@ class Navigator; class RTL : public MissionBlock { public: - /** - * Constructor - */ RTL(Navigator *navigator, const char *name); - /** - * Destructor - */ ~RTL(); - /** - * This function is called while the mode is inactive - */ - void on_inactive(); + virtual void on_inactive(); - /** - * This function is called while the mode is active - * - * @param position setpoint triplet that needs to be set - * @return true if updated - */ - bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: /** -- cgit v1.2.3 From 12be974bd67676ef243d069593b179108976da22 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 13:07:38 +0200 Subject: navigator: whitespace fix --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 395669698..ab99a6b7e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -255,7 +255,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(pos_sp_triplet); pos_sp_triplet->previous.valid = false; -- cgit v1.2.3 From 628477ee2c75d16758688f53c0f8d9f29457f831 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 14:01:58 +0200 Subject: Update estimator from Paul --- .../ekf_att_pos_estimator/estimator_23states.cpp | 610 ++++++++++++++++----- .../ekf_att_pos_estimator/estimator_23states.h | 20 +- .../ekf_att_pos_estimator/estimator_utilities.cpp | 19 + .../ekf_att_pos_estimator/estimator_utilities.h | 3 +- 4 files changed, 509 insertions(+), 143 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index e4b0c2b14..d77f4a6e3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -25,6 +25,7 @@ AttPosEKF::AttPosEKF() useAirspeed = true; useCompass = true; useRangeFinder = true; + useOpticalFlow = true; numericalProtection = true; refSet = false; storeIndex = 0; @@ -227,10 +228,22 @@ void AttPosEKF::CovariancePrediction(float dt) // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; processNoise[13] = dVelBiasSigma; - for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; - for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + if (!inhibitWindStates) { + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + } else { + for (uint8_t i=14; i<=15; i++) processNoise[i] = 0; + } + if (!inhibitMagStates) { + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + } else { + for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + } + if (!inhibitGndHgtState) { + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + } else { + processNoise[22] = 0; + } // square all sigmas for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); @@ -842,30 +855,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[i][i] = nextP[i][i] + processNoise[i]; } - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,16,21); - zeroCols(nextP,16,21); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,14,15); - zeroCols(nextP,14,15); - } - - // If on ground, inhibit terrain offset updates by - // setting the coresponding covariance terms to zero. - if (onGround) - { - zeroRows(nextP,22,22); - zeroCols(nextP,22,22); - } - // If the total position variance exceds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -882,48 +871,22 @@ void AttPosEKF::CovariancePrediction(float dt) } } - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i <= 13; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; } - } - ConstrainVariances(); + ConstrainVariances(); } void AttPosEKF::FuseVelposNED() @@ -944,7 +907,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit; + uint8_t indexLimit = 22; // declare variables used by state and covariance update calculations float velErr; @@ -981,11 +944,6 @@ void AttPosEKF::FuseVelposNED() R_OBS[4] = R_OBS[3]; R_OBS[5] = sq(posDSigma) + sq(posErr); - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } // calculate innovations and check GPS data validity using an innovation consistency check if (fuseVelData) { @@ -1071,15 +1029,6 @@ void AttPosEKF::FuseVelposNED() { fuseData[5] = true; } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 22; - } - else - { - indexLimit = 13; - } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { @@ -1113,6 +1062,22 @@ void AttPosEKF::FuseVelposNED() if (obsIndex != 5) { Kfusion[13] = 0; } + // Don't update wind states if inhibited + if (inhibitWindStates) { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + // Don't update magnetic field states if inhibited + if (inhibitMagStates) { + for (uint8_t i = 16; i<=21; i++) + { + Kfusion[i] = 0; + } + } + // Don't update terrain state if inhibited + if (inhibitGndHgtState) { + Kfusion[22] = 0; + } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1179,7 +1144,6 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1189,19 +1153,6 @@ void AttPosEKF::FuseMagnetometer() // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = n_states; - } - else - { - indexLimit = 13 + 1; - } - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - // Calculate observation jacobians and Kalman gains if (obsIndex == 0) { @@ -1287,15 +1238,31 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); - Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1342,15 +1309,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); - Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[1] = 1.0f/SK_MY[0]; innovMag[1] = MagPred[1] - magData.y; } @@ -1398,15 +1384,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); - Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; @@ -1416,7 +1421,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < indexLimit; j++) + for (uint8_t j= 0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1433,7 +1438,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1455,9 +1460,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1474,9 +1479,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1552,15 +1557,31 @@ void AttPosEKF::FuseAirspeed() Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + } else { + Kfusion[22] = 0; + } varInnovVtas = 1.0f/SK_TAS; // Calculate the measurement innovation @@ -1662,9 +1683,9 @@ void AttPosEKF::FuseRangeFinder() float ptd = statesAtRngTime[22]; // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sin(rngFinderPitch); + SH_RNG[4] = sinf(rngFinderPitch); cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && cosRngTilt > 0.87f) + if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations @@ -1685,10 +1706,12 @@ void AttPosEKF::FuseRangeFinder() SK_RNG[5] = SH_RNG[2]; Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[0]; + // Calculate the measurement innovation rngPred = (ptd - pd)/cosRngTilt; innovRng = rngPred - rngMea; - //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovRng*innovRng*SK_RNG[0]) < 25) @@ -1704,6 +1727,293 @@ void AttPosEKF::FuseRangeFinder() } +void AttPosEKF::FuseOptFlow() +{ + static uint8_t obsIndex; + static float SH_LOS[13]; + static float SKK_LOS[15]; + static float SK_LOS[2]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static Vector3f delAng; + static float R_LOS = 0.01f; + static float losPred[2]; + + // Transformation matrix from nav to body axes + Mat3f Tnb; + // Transformation matrix from body to sensor axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tbs; + Tbs.x.y = a3; + Tbs.y.x = -a3; + Tbs.x.z = -a2; + Tbs.z.x = a2; + Tbs.y.z = a1; + Tbs.z.y = -a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) + { + // Sequential fusion of XY components to spread processing load across + // two prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtOptFlowTime[0]; + q1 = statesAtOptFlowTime[1]; + q2 = statesAtOptFlowTime[2]; + q3 = statesAtOptFlowTime[3]; + vn = statesAtOptFlowTime[4]; + ve = statesAtOptFlowTime[5]; + vd = statesAtOptFlowTime[6]; + pd = statesAtOptFlowTime[9]; + ptd = statesAtOptFlowTime[22]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // calculate rotation from NED to body axes + float q00 = sq(q0); + float q11 = sq(q1); + float q22 = sq(q2); + float q33 = sq(q3); + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); + + // calculate transformation from NED to sensor axes + Tns = Tbs*Tnb; + + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + + // calculate delta angles in sensor axes + Vector3f delAngRel = Tbs*delAng; + + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); + //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); + //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SKK_LOS[14] = SH_LOS[0]; + + SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptFlowData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); + Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 3Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1904,6 +2214,24 @@ void AttPosEKF::OnGroundCheck() if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } + // don't update wind states if there is no airspeed measurement + if (onGround || !useAirspeed) { + inhibitWindStates = true; + } else { + inhibitWindStates =false; + } + // don't update magnetic field states if on ground or not using compass + if (onGround || !useCompass) { + inhibitMagStates = true; + } else { + inhibitMagStates = false; + } + // don't update terrain offset state if on ground + if (onGround) { + inhibitGndHgtState = true; + } else { + inhibitGndHgtState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -1931,8 +2259,8 @@ void AttPosEKF::CovarianceInit() P[11][11] = P[10][10]; P[12][12] = P[10][10]; P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(8.0f); - P[15][14] = P[14][14]; + P[14][14] = sq(0.0f); + P[15][15] = P[14][14]; P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index dc461cfa1..3110266ce 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -29,6 +29,10 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + float a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -55,6 +59,9 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -115,6 +122,7 @@ public: float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -147,9 +155,13 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float losData[2]; // optical flow LOS rate measurements (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation + float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) + float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2 float varInnovVtas; // innovation variance output + float varInnovRng; // range finder innovation variance float VtasMeas; // true airspeed measurement (m/s) float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) @@ -178,12 +190,18 @@ public: bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseRngData; ///< true when range data is fused + bool fuseOptFlowData; // true when optical flow data is fused + + bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant + bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used bool ekfDiverged; uint64_t lastReset; @@ -208,7 +226,7 @@ void FuseAirspeed(); void FuseRangeFinder(); -void FuseOpticalFlow(); +void FuseOptFlow(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index b4767a0d3..24168b84c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -101,6 +101,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index d47568b62..5648cb05c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -41,6 +41,7 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1); Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Mat3f operator*( Mat3f matIn1, Mat3f matIn2); Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*(Vector3f vecIn1, float sclIn1); @@ -79,4 +80,4 @@ struct ekf_status_report { bool velOffsetExcessive; }; -void ekf_debug(const char *fmt, ...); \ No newline at end of file +void ekf_debug(const char *fmt, ...); -- cgit v1.2.3 From 0bf9c2a9b262a4c8569031f0f7e9ded432d2d4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 14:09:22 +0200 Subject: navigator: API changes, reparing to move manual modes to navigator, WIP --- src/modules/navigator/loiter.cpp | 21 ++++++++----- src/modules/navigator/loiter.h | 4 +-- src/modules/navigator/mission.cpp | 42 ++++++++++++------------- src/modules/navigator/mission.h | 6 ++-- src/modules/navigator/mission_block.cpp | 53 +++++++++++++++----------------- src/modules/navigator/mission_block.h | 4 +-- src/modules/navigator/navigator.h | 4 ++- src/modules/navigator/navigator_main.cpp | 11 ++++--- src/modules/navigator/navigator_mode.cpp | 19 ++++++------ src/modules/navigator/navigator_mode.h | 9 ++---- 10 files changed, 87 insertions(+), 86 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 5e7067b0e..f827e70c9 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -36,6 +36,7 @@ * Helper class to loiter * * @author Julian Oes + * @author Anton Babushkin */ #include @@ -51,14 +52,13 @@ #include #include "loiter.h" +#include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name) { /* load initial params */ updateParams(); - /* initial reset */ - on_inactive(); } Loiter::~Loiter() @@ -71,16 +71,23 @@ Loiter::on_inactive() } void -Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Loiter::on_activation() { - set_loiter_item(pos_sp_triplet); + /* set current mission item to loiter */ + set_loiter_item(&_mission_item); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_active() { - return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 5ce86d2a7..37ab57a07 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -56,9 +56,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ab99a6b7e..1e86b1b6c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -54,8 +54,8 @@ #include #include -#include "navigator.h" #include "mission.h" +#include "navigator.h" Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), @@ -97,16 +97,14 @@ Mission::on_inactive() } void -Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_activation() { - set_mission_items(pos_sp_triplet); + set_mission_items(); } -bool -Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Mission::on_active() { - bool updated = false; - /* check if anything has changed */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -122,18 +120,18 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* reset mission items if needed */ if (onboard_updated || offboard_updated) { - set_mission_items(pos_sp_triplet); - updated = true; + set_mission_items(); + + _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); - set_mission_items(pos_sp_triplet); - updated = true; - } + set_mission_items(); - return updated; + _navigator->set_position_setpoint_triplet_updated(); + } } void @@ -223,8 +221,10 @@ Mission::advance_mission() } void -Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::set_mission_items() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + set_previous_pos_setpoint(pos_sp_triplet); /* try setting onboard mission item */ @@ -235,7 +235,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { @@ -245,7 +245,8 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + } else { if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), @@ -255,12 +256,15 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(pos_sp_triplet); + set_loiter_item(&_mission_item); + pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + reset_mission_item_reached(); report_mission_finished(); } @@ -279,7 +283,6 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -306,7 +309,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -330,7 +332,6 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } @@ -358,7 +359,6 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 38a4f7612..ac293990f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -71,9 +71,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** @@ -94,7 +94,7 @@ private: /** * Set new mission items */ - void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_mission_items(); /** * Try to set the current position setpoint from an onboard mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index acf3ad569..bf3c934a6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -58,8 +58,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _mission_item({0}), - _mission_item_valid(false) + _mission_item({0}) { } @@ -70,6 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { + return false; + } + if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _navigator->get_vstatus()->condition_landed; } @@ -83,7 +86,6 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { - float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; @@ -209,43 +211,38 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } void -MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_loiter_item(struct mission_item_s *item) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ - _mission_item.nav_cmd = NAV_CMD_IDLE; - - _navigator->set_can_loiter_at_sp(false); - - mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + item->nav_cmd = NAV_CMD_IDLE; } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { /* use current position setpoint */ - _mission_item.lat = pos_sp_triplet->current.lat; - _mission_item.lon = pos_sp_triplet->current.lon; - _mission_item.altitude = pos_sp_triplet->current.alt; + item->lat = pos_sp_triplet->current.lat; + item->lon = pos_sp_triplet->current.lon; + item->altitude = pos_sp_triplet->current.alt; } else { /* use current position */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; } - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4f79238f4..22f42cc31 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,11 +91,9 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead - void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_loiter_item(struct mission_item_s *item); mission_item_s _mission_item; - bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 184ecd365..5dcd8859e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,6 +103,7 @@ public: * Setters */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } /** * Getters @@ -110,6 +111,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } @@ -162,7 +164,7 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _update_triplet; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 266114e38..bb48db829 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -121,7 +121,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { @@ -371,20 +371,21 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { + // TODO publish empty sp only once _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _update_triplet = true; + _pos_sp_triplet_updated = true; } - if (_update_triplet) { + if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); - _update_triplet = false; + _pos_sp_triplet_updated = false; } perf_end(_loop_perf); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 6361ea9c8..f43215665 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -40,6 +40,7 @@ */ #include "navigator_mode.h" +#include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : SuperBlock(NULL, name), @@ -56,18 +57,17 @@ NavigatorMode::~NavigatorMode() { } -bool -NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { +void +NavigatorMode::run(bool active) { if (active) { if (_first_run) { /* first run */ _first_run = false; - on_activation(pos_sp_triplet); - return true; + on_activation(); } else { /* periodic updates when active */ - on_active(pos_sp_triplet); + on_active(); } } else { @@ -83,14 +83,13 @@ NavigatorMode::on_inactive() } void -NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_activation() { /* invalidate position setpoint by default */ - pos_sp_triplet->current.valid = false; + _navigator->get_position_setpoint_triplet()->current.valid = false; } -bool -NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +NavigatorMode::on_active() { - return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 5c36af1fe..a7ba79bba 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -66,7 +66,7 @@ public: */ virtual ~NavigatorMode(); - bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + void run(bool active); /** * This function is called while the mode is inactive @@ -76,15 +76,12 @@ public: /** * This function is called one time when mode become active, poos_sp_triplet must be initialized here */ - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); /** * This function is called while the mode is active - * - * @param position setpoint triplet to set - * @return true if position setpoint triplet has been changed */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); protected: Navigator *_navigator; -- cgit v1.2.3 From adf230ce4efe30087e751f143dab1df33a7d3b70 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 15:35:34 +0200 Subject: navigator: more API changes, duplicate code removed --- src/modules/navigator/mission.cpp | 166 ++++++++++++++------------------ src/modules/navigator/mission.h | 10 +- src/modules/navigator/mission_block.cpp | 5 +- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/rtl.cpp | 25 ++--- src/modules/navigator/rtl.h | 6 +- 6 files changed, 95 insertions(+), 119 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1e86b1b6c..fbfd1259d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -225,29 +225,27 @@ Mission::set_mission_items() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(pos_sp_triplet); + /* set previous position setpoint to current */ + set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + if (set_mission_item(true, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: onboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ - } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + } else if (set_mission_item(false, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: offboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); } else { + /* no mission available, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: mission finished"); @@ -257,126 +255,108 @@ Mission::set_mission_items() } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - + /* set loiter mission item */ set_loiter_item(&_mission_item); + /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + reset_mission_item_reached(); report_mission_finished(); + + _navigator->set_position_setpoint_triplet_updated(); + return; } -} -bool -Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) -{ - /* make sure param is up to date */ - updateParams(); - if (_param_onboard_enabled.get() > 0 && - _current_onboard_mission_index >= 0&& - _current_onboard_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, - &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + /* new current mission item set */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow - reset_mission_item_reached(); + /* try to read next mission item */ + struct mission_item_s mission_item_next; + if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - /* TODO: report this somehow */ - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; } - return false; + + _navigator->set_position_setpoint_triplet_updated(); } bool -Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) { - if (_current_offboard_mission_index >= 0 && - _current_offboard_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + /* make sure param is up to date */ + updateParams(); - reset_mission_item_reached(); + /* select onboard/offboard mission */ + int *mission_index_ptr; + struct mission_s *mission; + dm_item_t dm_item; + int mission_index_next; - report_current_offboard_mission_item(); - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; + if (onboard) { + /* onboard mission */ + if (!_param_onboard_enabled.get()) { + return false; } - } - return false; -} -void -Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - int next_temp_mission_index = _onboard_mission.current_index + 1; + mission_index_next = _current_onboard_mission_index + 1; + mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index; - /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; - } - } + mission = &_onboard_mission; - /* give up */ - next_pos_sp->valid = false; - return; -} + dm_item = DM_KEY_WAYPOINTS_ONBOARD; + + } else { + /* offboard mission */ + mission_index_next = _current_offboard_mission_index + 1; + mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + + mission = &_offboard_mission; -void -Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; - warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; } + } + + if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; + + if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) { + memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; } } - /* give up */ - next_pos_sp->valid = false; - return; + return false; } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr, struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { + for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), "#audio: ERROR waypoint could not be read"); @@ -394,7 +374,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (is_current) { (new_mission_item->do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ @@ -405,12 +385,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; + *mission_index_ptr = new_mission_item->do_jump_mission_index; } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ - (*mission_index)++; + (*mission_index_ptr)++; } } else { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ac293990f..7d07860de 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,16 +97,10 @@ private: void set_mission_items(); /** - * Try to set the current position setpoint from an onboard mission item + * Try to set given mission item from an offboard/onboard mission item * @return true if mission item successfully set */ - bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - - /** - * Try to set the current position setpoint from an offboard mission item - * @return true if mission item successfully set - */ - bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item); /** * Try to set the next position setpoint from an onboard mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index bf3c934a6..cbf7ac987 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -202,9 +202,10 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } void -MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_previous_pos_setpoint() { - /* reuse current setpoint as previous setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (pos_sp_triplet->current.valid) { memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 22f42cc31..adf50bc39 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -86,7 +86,7 @@ protected: /** * Set previous position setpoint to current setpoint */ - void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_previous_pos_setpoint(); /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7cf6bb1a8..6d7afcf4b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -82,7 +82,7 @@ RTL::on_inactive() } void -RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { @@ -105,28 +105,27 @@ RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) } } - set_rtl_item(pos_sp_triplet); + set_rtl_item(); } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_active() { if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); - set_rtl_item(pos_sp_triplet); - return true; + set_rtl_item(); } - - return false; } void -RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +RTL::set_rtl_item() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(pos_sp_triplet); + set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -273,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + reset_mission_item_reached(); + /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9c8b3fdfc..5928f1f07 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -63,15 +63,15 @@ public: virtual void on_inactive(); - virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** * Set the RTL item */ - void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + void set_rtl_item(); /** * Move to next RTL item -- cgit v1.2.3 From 72071cdcd39cae6be8533e67d01cbc7533ca6cb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 19:22:22 +0200 Subject: Fix failed merge --- src/modules/mavlink/mavlink_main.h | 54 -------------------------------------- 1 file changed, 54 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 18141972a..91d79057c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,14 +138,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } -<<<<<<< HEAD -======= - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 - static int start_helper(int argc, char *argv[]); /** @@ -165,15 +157,11 @@ public: */ int set_hil_enabled(bool hil_enabled); -<<<<<<< HEAD void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); -======= - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int get_instance_id(); @@ -228,7 +216,6 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; -<<<<<<< HEAD MavlinkMissionManager *_mission_manager; orb_advert_t _mission_pub; @@ -236,32 +223,12 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; -======= - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; - - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; pthread_t _receive_thread; -<<<<<<< HEAD - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; -======= - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -269,7 +236,6 @@ private: int _uart_fd; int _baudrate; int _datarate; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 /** * If the queue index is not at 0, the queue sending @@ -347,26 +313,9 @@ private: void mavlink_update_system(); -<<<<<<< HEAD uint8_t get_system_id(); uint8_t get_component_id(); -======= - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -381,11 +330,8 @@ private: int message_buffer_is_empty(); -<<<<<<< HEAD bool message_buffer_write(const void *ptr, int size); -======= ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From 43b9d96cf4306bc472d16956f5437dbef7c630c0 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 30 Jun 2014 01:55:40 +0200 Subject: Merged upstream master, add missing change --- src/modules/mavlink/mavlink_messages.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 472e2810d..06c2e737e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -669,16 +669,16 @@ protected: if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph), - cm_uint16_from_m_float(gps->epv), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_used); + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph), + cm_uint16_from_m_float(gps.epv), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_used); } } }; -- cgit v1.2.3 From e8c34fa174c6a47d5d2def2417c58beb846bb30f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 10:25:57 +0200 Subject: force setpoint uorb topic: add yaw field --- src/modules/uORB/topics/vehicle_force_setpoint.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index cd948985f..e3a7360b2 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -52,6 +52,7 @@ struct vehicle_force_setpoint_s { float x; /**< in N NED */ float y; /**< in N NED */ float z; /**< in N NED */ + float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ }; /**< Desired force in NED frame */ /** -- cgit v1.2.3 From a4315aee2ee69068c9a13568091805e20d0b04c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 10:47:08 +0200 Subject: navigator: mission mode fixed --- src/modules/navigator/mission.cpp | 63 ++++++++++++++++----------------------- src/modules/navigator/mission.h | 21 ++----------- 2 files changed, 27 insertions(+), 57 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fbfd1259d..f207776a9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,9 +70,9 @@ Mission::Mission(Navigator *navigator, const char *name) : { /* load initial params */ updateParams(); + /* set initial mission items */ on_inactive(); - } Mission::~Mission() @@ -121,16 +121,12 @@ Mission::on_active() /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } } @@ -223,13 +219,16 @@ Mission::advance_mission() void Mission::set_mission_items() { + /* make sure param is up to date */ + updateParams(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (set_mission_item(true, false, &_mission_item)) { + if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); @@ -237,7 +236,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (set_mission_item(false, false, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); @@ -284,7 +283,8 @@ Mission::set_mission_items() /* try to read next mission item */ struct mission_item_s mission_item_next; - if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) { + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); @@ -297,11 +297,8 @@ Mission::set_mission_items() } bool -Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) +Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { - /* make sure param is up to date */ - updateParams(); - /* select onboard/offboard mission */ int *mission_index_ptr; struct mission_s *mission; @@ -310,12 +307,8 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m if (onboard) { /* onboard mission */ - if (!_param_onboard_enabled.get()) { - return false; - } - mission_index_next = _current_onboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index; + mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; mission = &_onboard_mission; @@ -324,7 +317,7 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } else { /* offboard mission */ mission_index_next = _current_offboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; mission = &_offboard_mission; @@ -336,27 +329,20 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } } - if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { - struct mission_item_s new_mission_item; - - if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) { - memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; } - return false; -} -bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr, - struct mission_item_s *new_mission_item) -{ /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); + /* read mission item to temp storage first to not overwrite current mission item if data damaged */ + struct mission_item_s mission_item_tmp; + /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), "#audio: ERROR waypoint could not be read"); @@ -364,18 +350,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { /* do DO_JUMP as many times as requested */ - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { - (new_mission_item->do_jump_current_count)++; + (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), @@ -385,7 +370,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = new_mission_item->do_jump_mission_index; + *mission_index_ptr = mission_item_tmp.do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); @@ -395,6 +381,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } else { /* if it's not a DO_JUMP, then we were successful */ + memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 7d07860de..29e4d41f6 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,27 +97,10 @@ private: void set_mission_items(); /** - * Try to set given mission item from an offboard/onboard mission item - * @return true if mission item successfully set - */ - bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item); - - /** - * Try to set the next position setpoint from an onboard mission item - */ - void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Try to set the next position setpoint from an offboard mission item - */ - void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Read a mission item from the dataman and watch out for DO_JUMPS + * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful */ - bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, - struct mission_item_s *new_mission_item); + bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item); /** * Report that a mission item has been reached -- cgit v1.2.3 From 59775efaad782a1e4f5e346832407c71f1355c89 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:02:29 +0200 Subject: navigator: takeoff on start of mission implemented --- src/modules/navigator/mission.cpp | 133 ++++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 3 + 2 files changed, 109 insertions(+), 27 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f207776a9..2e5047e2c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -60,13 +60,16 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), + _param_takeoff_alt(this, "TAKEOFF_ALT"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _need_takeoff(true), + _takeoff(false) { /* load initial params */ updateParams(); @@ -94,6 +97,10 @@ Mission::on_inactive() if (offboard_updated) { update_offboard_mission(); } + + if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + } } void @@ -127,6 +134,12 @@ Mission::on_active() if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(); + + } else { + /* if waypoint position reached allow loiter on the setpoint */ + if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { + _navigator->set_can_loiter_at_sp(true); + } } } @@ -148,6 +161,7 @@ Mission::update_onboard_mission() } /* otherwise, just leave it */ } + } else { _onboard_mission.count = 0; _onboard_mission.current_index = 0; @@ -164,10 +178,12 @@ Mission::update_offboard_mission() if (_offboard_mission.current_index >= 0 && _offboard_mission.current_index < (int)_offboard_mission.count) { _current_offboard_mission_index = _offboard_mission.current_index; + } else { /* if less WPs available, reset to first WP */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ } else if (_current_offboard_mission_index < 0) { _current_offboard_mission_index = 0; @@ -181,6 +197,7 @@ Mission::update_offboard_mission() if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } @@ -189,11 +206,13 @@ Mission::update_offboard_mission() (size_t)_offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); + } else { _offboard_mission.count = 0; _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -201,18 +220,23 @@ Mission::update_offboard_mission() void Mission::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; + if (_takeoff) { + _takeoff = false; + + } else { + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } } } @@ -271,26 +295,81 @@ Mission::set_mission_items() return; } - /* new current mission item set */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); + /* do takeoff on first waypoint for rotary wing vehicles */ + if (_navigator->get_vstatus()->is_rotary_wing) { + /* force takeoff if landed (additional protection) */ + if (!_takeoff && _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + } - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + /* new current mission item set, check if we need takeoff */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + _takeoff = true; + _need_takeoff = false; + } } - // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_takeoff) { + /* do takeoff before going to setpoint */ + /* set mission item as next position setpoint */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + /* calculate takeoff altitude */ + float takeoff_alt = _mission_item.altitude; + if (_mission_item.altitude_is_relative) { + takeoff_alt += _navigator->get_home_position()->alt; + } + + if (_navigator->get_vstatus()->condition_landed) { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); + + } else { + takeoff_alt = _navigator->get_home_position()->alt + _param_takeoff_alt.get(); + } + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow + + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 29e4d41f6..d4808b6f4 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -123,12 +123,15 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; + control::BlockParamFloat _param_takeoff_alt; struct mission_s _onboard_mission; struct mission_s _offboard_mission; int _current_onboard_mission_index; int _current_offboard_mission_index; + bool _need_takeoff; + bool _takeoff; orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; -- cgit v1.2.3 From 0a159e1a2490e5ec7f368d938e2b0fd365d2731e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:31:29 +0200 Subject: mavlink, commander: bugs fixed --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 10 ++-------- 2 files changed, 3 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ff1195c9..bba14ea27 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,7 +1407,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b4184b21c..67ef8d00f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: int send_statustext(const char *string); int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -193,7 +193,7 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -322,10 +322,6 @@ private: void mavlink_update_system(); - uint8_t get_system_id(); - - uint8_t get_component_id(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); @@ -338,8 +334,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(const void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From f0cfa04b852212cd0be4bc1bffbc4eece54ce659 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 20:55:02 +0200 Subject: mtecs blocks: fix warning --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..4f18d36e8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -72,8 +72,8 @@ public: * @return: true if the limit is applied, false otherwise */ bool limit(float& value, float& difference) { - float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); - float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); if (value < minimum) { difference = value - minimum; value = minimum; @@ -86,7 +86,7 @@ public: return false; } //accessor: - bool isAngularLimit() {return _isAngularLimit ;} + bool getIsAngularLimit() {return _isAngularLimit ;} float getMin() { return _min.get(); } float getMax() { return _max.get(); } void setMin(float value) { _min.set(value); } -- cgit v1.2.3 From 57827563b9b3b8771be28d580842d4fbcf084918 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:11:05 +0200 Subject: fw pos ctrl l1 main: fix warning --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 38 ++++++++++------------ 1 file changed, 18 insertions(+), 20 deletions(-) (limited to 'src/modules') 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 000c02e3d..e877f0e84 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 @@ -414,11 +414,23 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), +/* states */ + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), -/* states */ _loiter_hold(false), + _launch_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -427,24 +439,17 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), usePreTakeoffThrust(false), last_manual(false), + landingslope(), flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), + _airspeed_last_valid(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), + _l1_control(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -806,13 +811,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - - /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; - + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -945,7 +944,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); -- cgit v1.2.3 From 02db53cea57d1e2c643b6efc1c357afa3e6be20e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:13:29 +0200 Subject: fw pos ctrl l1 main: initialize all subscriptions --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') 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 e877f0e84..5ec49b6f8 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 @@ -129,7 +129,6 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -408,6 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _sensor_combined_sub(-1), _range_finder_sub(-1), /* publications */ -- cgit v1.2.3 From 173da25c9a55185377870f703cd751fd44b902f0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:16:32 +0200 Subject: fw pos ctrl l1 main: remove unused modes --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 83 ---------------------- 1 file changed, 83 deletions(-) (limited to 'src/modules') 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 5ec49b6f8..fc0d588d2 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 @@ -1133,89 +1133,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* posctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** POSCTRL FLIGHT **/ - - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - //XXX not used - - /* climb out control */ -// bool climb_out = false; -// -// /* user wants to climb out */ -// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { -// climb_out = true; -// } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - - } else if (0/* altctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** ALTCTRL FLIGHT **/ - - if (0/* switched from another mode to altctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - /* user switched off throttle */ - if (_manual.z < 0.1f) { - throttle_max = 0.0f; - } - - /* climb out control */ - bool climb_out = false; - - /* user wants to climb out */ - if (_manual.x > 0.3f && _manual.z > 0.8f) { - climb_out = true; - } - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _manual.y; - _att_sp.yaw_body = _manual.r; - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed); - } else { _was_pos_control_mode = false; -- cgit v1.2.3 From 62a92542891d32d88c1993e03da9a17d59f70d04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:22:56 +0200 Subject: fw pos ctrl l1 main: remove several unused vars --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 30 ---------------------- 1 file changed, 30 deletions(-) (limited to 'src/modules') 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 fc0d588d2..445abf23e 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 @@ -152,18 +152,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - /** manual control states */ - float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ - double _loiter_hold_lat; - double _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; - - double _launch_lat; - double _launch_lon; - float _launch_alt; - bool _launch_valid; - /* land states */ /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; @@ -429,8 +417,6 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), - _loiter_hold(false), - _launch_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -609,13 +595,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() bool was_armed = _control_mode.flag_armed; orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - - if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat; - _launch_lon = _global_pos.lon; - _launch_alt = _global_pos.alt; - _launch_valid = true; - } } } @@ -1109,15 +1088,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { reset_landing_state(); -- cgit v1.2.3 From 527ce1048a27f0d15e99a4ba899897c06a8cb3f2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:25:18 +0200 Subject: fw pos ctrl l1 main: init target_bearing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') 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 445abf23e..56fe65f9f 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 @@ -168,8 +168,8 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_rel_last; + /* heading hold */ float target_bearing; @@ -427,6 +427,7 @@ FixedwingPositionControl::FixedwingPositionControl() : last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), + target_bearing(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), -- cgit v1.2.3 From c782d5d3796f9f936c30de2b0395ba76b42b8795 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:27:05 +0200 Subject: fw pos ctrl l1 main: remove never used var --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules') 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 56fe65f9f..38d275eb0 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 @@ -593,8 +593,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() if (vstatus_updated) { - bool was_armed = _control_mode.flag_armed; - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } -- cgit v1.2.3 From acca14673c4934ea8cdca46286db6d769c4dca40 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:31:50 +0200 Subject: fw pos ctrl l1 main: remove code with no effect --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 --------- 1 file changed, 9 deletions(-) (limited to 'src/modules') 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 38d275eb0..d07258094 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 @@ -592,7 +592,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_check(_control_mode_sub, &vstatus_updated); if (vstatus_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } @@ -1217,14 +1216,6 @@ FixedwingPositionControl::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); -- cgit v1.2.3 From 45433c3711dba5c972e02e8c9263853164670a86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:18 +0200 Subject: Hotfix: Fix use of circuit breaker param --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b59531d69..8f697749e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -86,7 +86,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; - (void)param_get(param_find(breaker), val); + (void)param_get(param_find(breaker), &val); return (val == magic); } -- cgit v1.2.3 From fb98251e4d08d351c7ba404e3816958435c57515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:36 +0200 Subject: Warning fixes in commander --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && (status->avionics_power_rail_voltage < 4.9f)) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } @@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); -- cgit v1.2.3 From 6042999aab26276ff87e7274b97b69200919b86f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 11:59:25 +0200 Subject: Add vital comment to circuit breaker --- src/modules/systemlib/circuit_breaker.h | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src/modules') diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 86439e2a5..1175dbce8 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -40,6 +40,15 @@ #ifndef CIRCUIT_BREAKER_H_ #define CIRCUIT_BREAKER_H_ +/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING + * + * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE, + * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS. + * http://pixhawk.org/dev/circuit_breakers + * + * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE + * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT. + */ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 -- cgit v1.2.3 From 8acbe6d5b6770e92fdcb86ba268492217d3e26bd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Tue, 1 Jul 2014 14:08:59 -0700 Subject: Added class to convert gnss message from uavcan to uorb --- src/modules/uavcan/gnss_receiver.cpp | 122 +++++++++++++++++++++++++++++++++++ src/modules/uavcan/gnss_receiver.hpp | 86 ++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 7 +- src/modules/uavcan/uavcan_main.hpp | 2 + 4 files changed, 216 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/gnss_receiver.cpp create mode 100644 src/modules/uavcan/gnss_receiver.hpp (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp new file mode 100644 index 000000000..e92468333 --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 gnss_receiver.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss_receiver.hpp" +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : + _node(node), + _uavcan_sub_status(node), + _report_pub(-1) +{ +} + +int UavcanGnssReceiver::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + return res; +} + +void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + + /* Use Jacobian to transform velocity covariance to heading covariance + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * var(heading) = J(velocity)*var(velocity)*J(velocity)^T + */ + _report.c_variance_rad = + msg.ned_velocity[1] * msg.ned_velocity[1] * msg.velocity_covariance[0] + + -2*msg.ned_velocity[1] * msg.ned_velocity[0] * msg.velocity_covariance[1] + + msg.ned_velocity[0] * msg.ned_velocity[0] * msg.velocity_covariance[4]; + + _report.fix_type = msg.status; + + _report.eph_m = sqrtf(_report.p_variance_m); + _report.epv_m = sqrtf(msg.position_covariance[8]); + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = msg.timestamp.husec * msg.timestamp.USEC_PER_LSB; // Convert to microseconds + + _report.timestamp_satellites = _report.timestamp_position; + _report.satellites_visible = msg.sats_used; + _report.satellite_info_available = 0; // Set to 0 for no info available + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp new file mode 100644 index 000000000..abb8a821a --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 gnss_receiver.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +class UavcanGnssReceiver +{ +public: + UavcanGnssReceiver(uavcan::INode& node); + + int init(); + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + typedef uavcan::MethodBinder&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ab687a6b9..5bc437670 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -63,7 +63,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node) + _esc_controller(_node), + _gnss_receiver(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -186,6 +187,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; + ret = _gnss_receiver.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 751a94a8a..126d44137 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -43,6 +43,7 @@ #include #include "esc_controller.hpp" +#include "gnss_receiver.hpp" /** * @file uavcan_main.hpp @@ -108,6 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; -- cgit v1.2.3 From 4cd66a3242aa2dfb03a3e58ffcc8d27e1350b7ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 00:25:19 +0200 Subject: Fix sdlog2 GPS time copy for log_on_start situation --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9b071ff99..f99aec34e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1070,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } -- cgit v1.2.3 From 722fe924255d5fdfbba407143405da7578b0dad7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 2 Jul 2014 16:45:42 +0200 Subject: Fix compile error: rename field in rc_channels.h --- src/modules/uORB/topics/rc_channels.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 829d8e57d..8978de471 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -56,7 +56,7 @@ enum RC_CHANNELS_FUNCTION { RETURN, POSCTL, LOITER, - OFFBOARD_MODE, + OFFBOARD, ACRO, FLAPS, AUX_1, -- cgit v1.2.3 From 6c6de9395818717916bbc1077fecd51c4db87936 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 10:04:07 -0700 Subject: Fixed heading covariance calculation and build errors. --- src/modules/uavcan/gnss_receiver.cpp | 17 ++++++++++++----- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index e92468333..490e35bd1 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -80,17 +80,24 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 2 Jul 2014 11:18:30 -0700 Subject: Fixed bug with zero-sized covariance arrays --- src/modules/uavcan/gnss_receiver.cpp | 77 +++++++++++++++++++++++------------- src/modules/uavcan/uavcan_main.cpp | 23 +++++------ 2 files changed, 61 insertions(+), 39 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 490e35bd1..23c221565 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -42,12 +42,13 @@ #include "gnss_receiver.hpp" #include -#define MM_PER_CM 10 // Millimeters per centimeter +#define MM_PER_CM 10 // Millimeters per centimeter +#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : - _node(node), - _uavcan_sub_status(node), - _report_pub(-1) +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) { } @@ -77,32 +78,52 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); + bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && + msg.velocity_covariance[0] > 0); + + if (valid_position_covariance) { + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + _report.eph_m = sqrtf(_report.p_variance_m); + } else { + _report.p_variance_m = -1.0; + _report.eph_m = -1.0; + } + + if (valid_velocity_covariance) { + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * msg.velocity_covariance[0] + + -2 * vel_n * vel_e * msg.velocity_covariance[1] + // Covariance matrix is symmetric + vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + _report.epv_m = sqrtf(msg.position_covariance[8]); - _report.eph_m = sqrtf(_report.p_variance_m); - _report.epv_m = sqrtf(msg.position_covariance[8]); + } else { + _report.s_variance_m_s = -1.0; + _report.c_variance_rad = -1.0; + _report.epv_m = -1.0; + } + + _report.fix_type = msg.status; _report.timestamp_velocity = _report.timestamp_position; _report.vel_n_m_s = msg.ned_velocity[0]; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc437670..c0c07be53 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,21 +243,22 @@ int UavcanNode::run() _node.setStatusOk(); + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -271,7 +272,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { -- cgit v1.2.3 From 607b6511a413b5bc2b2b0ae350a9451e83da9803 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 11:27:49 -0700 Subject: Fixed comments --- src/modules/uavcan/gnss_receiver.hpp | 6 ++---- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp index abb8a821a..18df8da2f 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -34,10 +34,8 @@ /** * @file gnss_receiver.hpp * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix * * @author Pavel Kirienko * @author Andrew Chambers diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 126d44137..443525379 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -109,7 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; -- cgit v1.2.3 From 76aa871a7125c98e79db4dde42de7863a24a762c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 00:26:33 +0200 Subject: estimator: Fixed body / world matrix initialization and reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..deaaf55fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions -- cgit v1.2.3 From 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 19:06:30 -0700 Subject: Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num --- src/modules/uavcan/gnss_receiver.cpp | 28 +++++++++++++++------------- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++------------ 2 files changed, 26 insertions(+), 25 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 23c221565..65a7b4a2a 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -43,7 +43,6 @@ #include #define MM_PER_CM 10 // Millimeters per centimeter -#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : _node(node), @@ -75,18 +74,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); - bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && - msg.velocity_covariance[0] > 0); + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); if (valid_position_covariance) { - _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + _report.p_variance_m = std::max(pos_cov[0], pos_cov[4]); _report.eph_m = sqrtf(_report.p_variance_m); } else { _report.p_variance_m = -1.0; @@ -94,7 +94,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c0c07be53..5bc437670 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,22 +243,21 @@ int UavcanNode::run() _node.setStatusOk(); - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -272,7 +271,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; + unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { -- cgit v1.2.3 From 7be2e0f13605eb9bb34eb04745a0e7a3936d94e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:18:00 +0200 Subject: Hotfix: Typo in disabled code path --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 6c0e876d1..60dcc62e0 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowvyslog(fmt, args); + lowsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From 04cca73baac0b027451a76d50b1f3b365b1feeef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:26:26 +0200 Subject: Hotfix of the Hotfix 8) --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 60dcc62e0..998b5ac7d 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowsyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From 2ec635dd1c33ef453818bd9396e3cec42694a30f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 13:30:20 +0200 Subject: Update estimator --- .../ekf_att_pos_estimator/estimator_23states.cpp | 32 +++++++++++----------- .../ekf_att_pos_estimator/estimator_utilities.cpp | 1 + 2 files changed, 17 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 393cf4c5a..973de1382 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1747,7 +1747,7 @@ void AttPosEKF::FuseOptFlow() static float losPred[2]; // Transformation matrix from nav to body axes - Mat3f Tnb; + Mat3f Tnb_local; // Transformation matrix from body to sensor axes // assume camera is aligned with Z body axis plus a misalignment // defined by 3 small angles about X, Y and Z body axis @@ -1764,7 +1764,7 @@ void AttPosEKF::FuseOptFlow() for (uint8_t i = 0; i < n_states; i++) { H_LOS[i] = 0.0f; } - Vector3f velNED; + Vector3f velNED_local; Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. @@ -1786,9 +1786,9 @@ void AttPosEKF::FuseOptFlow() vd = statesAtOptFlowTime[6]; pd = statesAtOptFlowTime[9]; ptd = statesAtOptFlowTime[22]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; // calculate rotation from NED to body axes float q00 = sq(q0); @@ -1801,24 +1801,24 @@ void AttPosEKF::FuseOptFlow() float q12 = q1 * q2; float q13 = q1 * q3; float q23 = q2 * q3; - Tnb.x.x = q00 + q11 - q22 - q33; - Tnb.y.y = q00 - q11 + q22 - q33; - Tnb.z.z = q00 - q11 - q22 + q33; - Tnb.y.x = 2*(q12 - q03); - Tnb.z.x = 2*(q13 + q02); - Tnb.x.y = 2*(q12 + q03); - Tnb.z.y = 2*(q23 - q01); - Tnb.x.z = 2*(q13 - q02); - Tnb.y.z = 2*(q23 + q01); + Tnb_local.x.x = q00 + q11 - q22 - q33; + Tnb_local.y.y = q00 - q11 + q22 - q33; + Tnb_local.z.z = q00 - q11 - q22 + q33; + Tnb_local.y.x = 2*(q12 - q03); + Tnb_local.z.x = 2*(q13 + q02); + Tnb_local.x.y = 2*(q12 + q03); + Tnb_local.z.y = 2*(q23 - q01); + Tnb_local.x.z = 2*(q13 - q02); + Tnb_local.y.z = 2*(q23 + q01); // calculate transformation from NED to sensor axes - Tns = Tbs*Tnb; + Tns = Tbs*Tnb_local; // calculate range from ground plain to centre of sensor fov assuming flat earth float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; + relVelSensor = Tns*velNED_local; // calculate delta angles in sensor axes Vector3f delAngRel = Tbs*delAng; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 24168b84c..29a8c8d1e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -8,6 +8,7 @@ #ifdef EKF_DEBUG #include +#include static void ekf_debug_print(const char *fmt, va_list args) -- cgit v1.2.3 From 1cca3ca8a5469e66dbb5bebbe518b55e4af426d8 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 3 Jul 2014 13:33:29 +0200 Subject: Use NAV-PVT with ubx7 and ubx8 modules This replaces NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC. --- src/drivers/gps/gps.cpp | 1 - src/drivers/gps/ubx.cpp | 125 +++++++++++++++++++++---- src/drivers/gps/ubx.h | 10 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/uORB/topics/vehicle_gps_position.h | 1 - 5 files changed, 113 insertions(+), 25 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 241e3bdf3..401b65dd4 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -299,7 +299,6 @@ GPS::task_main() _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.p_variance_m = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 44434a1df..d0854f5e9 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_velned(false), _disable_cmd_last(0), _ack_waiting_msg(0), - _ubx_version(0) + _ubx_version(0), + _use_nav_pvt(false) { decode_init(); } @@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate) /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + /* try to set rate for NAV-PVT */ + /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ + configure_message_rate(UBX_MSG_NAV_PVT, 1); if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + _use_nav_pvt = false; + } else { + _use_nav_pvt = true; } + UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (!_use_nav_pvt) { + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - configure_message_rate(UBX_MSG_NAV_VELNED, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -454,11 +462,23 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_HANDLE; // handle by default switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (!_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + break; + case UBX_MSG_NAV_POSLLH: if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SOL: @@ -466,6 +486,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_TIMEUTC: @@ -473,6 +495,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SVINFO: @@ -489,6 +513,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_MON_VER: @@ -675,6 +701,68 @@ UBX::payload_rx_done(void) // handle message switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; + + _gps_position->lat = _buf.payload_rx_nav_pvt.lat; + _gps_position->lon = _buf.payload_rx_nav_pvt.lon; + _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; + + _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; + _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; + + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; + _gps_position->vel_ned_valid = true; + + _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; + timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; + timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; + timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; + time_t epoch = mktime(&timeinfo); + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + clock_settime(CLOCK_REALTIME, &ts); +#endif + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + } + + _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->timestamp_velocity = hrt_absolute_time(); + _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_vel++; + _rate_count_lat_lon++; + + _got_posllh = true; + _got_velned = true; + + ret = 1; + break; + case UBX_MSG_NAV_POSLLH: UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); @@ -697,7 +785,6 @@ UBX::payload_rx_done(void) _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m - _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 65f2dd2ba..219a5762a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -183,7 +183,7 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; -/* Rx NAV-PVT */ +/* Rx NAV-PVT (ubx8) */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ uint16_t year; /**< Year (UTC)*/ @@ -215,9 +215,11 @@ typedef struct { uint16_t pDOP; /**< Position DOP [0.01] */ uint16_t reserved2; uint32_t reserved3; - int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */ - uint32_t reserved4; + int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; /**< (ubx8+ only) */ } ubx_payload_rx_nav_pvt_t; +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) /* Rx NAV-TIMEUTC */ typedef struct { @@ -395,6 +397,7 @@ typedef struct { /* General message and payload buffer union */ typedef union { + ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; ubx_payload_rx_nav_sol_t payload_rx_nav_sol; ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; @@ -533,6 +536,7 @@ private: uint16_t _ack_waiting_msg; ubx_buf_t _buf; uint32_t _ubx_version; + bool _use_nav_pvt; }; #endif /* UBX_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53a1638a3..6d361052c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index dbd59f4f3..80d65cd69 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ -- cgit v1.2.3 From d7394c7ef973e34d87187420444baad6fcf9854b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Jul 2014 19:00:22 +0200 Subject: mavlink: stack size increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cf6265f09..f73a58fa2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1601,7 +1601,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 3 Jul 2014 11:32:27 -0700 Subject: Using proper math library. Corrected speed variance calculation --- src/modules/uavcan/gnss_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 65a7b4a2a..3e98bdf14 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -41,6 +41,7 @@ #include "gnss_receiver.hpp" #include +#include #define MM_PER_CM 10 // Millimeters per centimeter @@ -86,7 +87,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Fri, 4 Jul 2014 15:49:02 +0200 Subject: navigator: takeoff altitude fixed --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2e5047e2c..cf6a764bf 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -326,11 +326,12 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } + /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); } else { - takeoff_alt = _navigator->get_home_position()->alt + _param_takeoff_alt.get(); + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); -- cgit v1.2.3 From e97161e96b8de4031746556ab4bd4f96d24a30c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Jul 2014 15:53:26 +0200 Subject: mavlink: stack size for main thread increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026a4d6c9..8dfa32ce8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2238,7 +2238,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From d4eae37e478860a59e21f3caceb3d8fc28f9fa7c Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Fri, 4 Jul 2014 21:00:05 -0400 Subject: Commander: Only consider latest active data link heartbeat for timeout. --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..699ced1ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; + hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; - } + if (telemetry.heartbeat_time >= latest_heartbeat) { + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } - } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + /* Only consider data link with most recent heartbeat */ + latest_heartbeat = telemetry.heartbeat_time; + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; + } } } -- cgit v1.2.3 From b9299e68d4147845bab9ed99509b3e50b7c94ae1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 13:35:12 -0700 Subject: Compiler warning fixes --- src/drivers/blinkm/blinkm.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++------- src/modules/commander/commander.cpp | 8 ++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 4 ++-- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/systemlib.c | 3 +++ src/systemcmds/param/param.c | 5 +++-- 8 files changed, 33 insertions(+), 28 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 92e089217..6b14f5945 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) : safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), - t_led_color({0}), + t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 212e33ff8..230be9281 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -297,12 +297,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // TO DO - this is messed up and won't compile float start_disp_x = radius * sinf(arc_start_bearing); float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; + float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float lon_start = (float)lon_now + start_disp_x / 111111.0f; + float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = (float)lon_now + end_disp_x / 111111.0f; + float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -319,7 +319,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..3bf3c857d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -472,7 +472,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); } else { @@ -634,7 +634,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - char *main_states_str[MAIN_STATE_MAX]; + const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; @@ -643,7 +643,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; - char *arming_states_str[ARMING_STATE_MAX]; + const char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; @@ -652,7 +652,7 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - char *nav_states_str[NAVIGATION_STATE_MAX]; + const char *nav_states_str[NAVIGATION_STATE_MAX]; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; 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 000c02e3d..a9556f453 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 @@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..c22e60ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock { public: // methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : SuperBlock(parent, name), - _isAngularLimit(isAngularLimit), + _isAngularLimit(fAngularLimit), _min(this, "MIN"), _max(this, "MAX") {}; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index ccc516f39..7aa2f3a5f 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT __EXPORT struct system_load_s system_load; -extern FAR struct _TCB *sched_gettcb(pid_t pid); +extern FAR struct tcb_s *sched_gettcb(pid_t pid); void cpuload_initialize_once() { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 9fff3eb88..90d8dd77c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -54,6 +54,9 @@ #include "systemlib.h" +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + void systemreset(bool to_bootloader) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index f8bff2f6f..235ab7186 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param) if (!(arg == NULL)) { /* start search */ - char *ss = search_string; - char *pp = p_name; + const char *ss = search_string; + const char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ -- cgit v1.2.3 From 43a1c1b5f600d5608a861d35d539319de1170923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 15:33:54 +0200 Subject: Code style improvement, fix linter warning --- src/modules/uavcan/esc_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index bde4d2a7f..f603d9448 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -50,10 +50,8 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : int UavcanEscController::init() { - int res = -1; - // ESC status subscription - res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); if (res < 0) { warnx("ESC status sub failed %i", res); -- cgit v1.2.3 From 2669f7f3af65921d4abbf3850cd62e48f2eeeec7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 15:34:50 +0200 Subject: Fix mixer limiter to never output min for an input of max + 1 quantum --- src/modules/uavcan/uavcan_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc437670..27e77e9c5 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -301,9 +301,7 @@ int UavcanNode::run() // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + if (!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 @@ -311,6 +309,18 @@ int UavcanNode::run() */ outputs.output[i] = -1.0f; } + + // limit outputs to valid range + + // never go below min + if (outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } + + // never go below max + if (outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } } // Output to the bus -- cgit v1.2.3 From bd5d3ebf70dc9e1aef106b60a840c17824d35b9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 16:08:37 +0200 Subject: telemetry_statur: use 4 separate topics --- src/modules/commander/commander.cpp | 87 +++++++++++++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 86 +++++++++++++++-------------- src/modules/sdlog2/sdlog2.c | 31 ++++++----- src/modules/sdlog2/sdlog2_format.h | 8 +++ src/modules/sdlog2/sdlog2_messages.h | 35 +++++++----- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/telemetry_status.h | 14 ++++- 7 files changed, 166 insertions(+), 100 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 699ced1ab..0003ec106 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - /* Subscribe to telemetry status */ - int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - struct telemetry_status_s telemetry; - memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to telemetry status topics */ + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_last_heartbeat[i] = 0; + telemetry_lost[i] = true; + } /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; - bool system_checked = false; while (!thread_should_exit) { @@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } - /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && - (telemetry.heartbeat_time > 0) && - (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { - - (void)rc_calibration_check(mavlink_fd); - system_checked = true; - } - orb_check(sp_man_sub, &updated); if (updated) { @@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(telemetry_sub, &updated); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + orb_check(telemetry_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + if (updated) { + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + + orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + + /* perform system checks when new telemetry link connected */ + if (mavlink_fd && + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + + (void)rc_calibration_check(mavlink_fd); + } + + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } orb_check(sensor_sub, &updated); @@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[]) } } - /* data link check */ - if (telemetry.heartbeat_time >= latest_heartbeat) { - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* data links check */ + bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; + if (telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + telemetry_lost[i] = false; } - - /* Only consider data link with most recent heartbeat */ - latest_heartbeat = telemetry.heartbeat_time; + have_link = true; } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + if (!telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + telemetry_lost[i] = true; } } } + if (have_link) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + status.data_link_lost = true; + status_changed = true; + } + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6d361052c..60da9c47d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; + } } void @@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39e5b6c41..0d36fa2c6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; + struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; @@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; int tecs_status_sub; @@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8c05e87c5..08a87e179 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -276,18 +276,7 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; - +// ID 22 available // ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ @@ -385,6 +374,23 @@ struct log_EST1_s { float s[16]; }; +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 687fc1d4a..9b118205e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index e9e00d76c..c4b99d520 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -72,6 +72,18 @@ struct telemetry_status_s { * @} */ -ORB_DECLARE(telemetry_status); +ORB_DECLARE(telemetry_status_0); +ORB_DECLARE(telemetry_status_1); +ORB_DECLARE(telemetry_status_2); +ORB_DECLARE(telemetry_status_3); + +#define TELEMETRY_STATUS_ORB_ID_NUM 4 + +static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { + ORB_ID(telemetry_status_0), + ORB_ID(telemetry_status_1), + ORB_ID(telemetry_status_2), + ORB_ID(telemetry_status_3), +}; #endif /* TOPIC_TELEMETRY_STATUS_H */ -- cgit v1.2.3 From 54b55c37c7ffec5c340f100d2027e91971967793 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 17:33:50 +0200 Subject: Reduce excessive stack sizes for FW estimation / control apps --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') 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 e4f805dc0..5d768b73d 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 @@ -1533,7 +1533,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 6000, + 5000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); 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 000c02e3d..6c86544fa 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 @@ -1449,7 +1449,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4048, + 3500, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); -- cgit v1.2.3 From 29eab09912ef38eca75b926d5006c0c59b4082b7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 17:58:21 +0200 Subject: sdlog2: TEL message format fixed --- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 08a87e179..87f7f718f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -438,10 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), -- cgit v1.2.3 From d2a2297a14bf2f4dbb71a6ed20c76617e93b14a2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 6 Jul 2014 11:24:39 -0700 Subject: Fixes #1130 --- src/modules/px4iofirmware/i2c.c | 3 +++ src/modules/px4iofirmware/px4io.h | 4 ++-- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 76762c0fc..6d1d1fc2d 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -64,12 +64,15 @@ #define rCCR REG(STM32_I2C_CCR_OFFSET) #define rTRISE REG(STM32_I2C_TRISE_OFFSET) +void i2c_reset(void); static int i2c_interrupt(int irq, void *context); static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +#ifdef DEBUG static void i2c_dump(void); +#endif static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ca175bfbc..b00a96717 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -extern bool sbus1_output(uint16_t *values, uint16_t num_values); -extern bool sbus2_output(uint16_t *values, uint16_t num_values); +extern void sbus1_output(uint16_t *values, uint16_t num_values); +extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 50108ea1b..b37259997 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { #define SELECT_PAGE(_page_name) \ do { \ - *values = &_page_name[0]; \ + *values = (uint16_t *)&_page_name[0]; \ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ } while(0) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 70ccab180..0f0414148 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -116,14 +116,14 @@ sbus_init(const char *device) return sbus_fd; } -bool +void sbus1_output(uint16_t *values, uint16_t num_values) { char a = 'A'; write(sbus_fd, &a, 1); } -bool +void sbus2_output(uint16_t *values, uint16_t num_values) { char b = 'B'; -- cgit v1.2.3 From 06f08ad04d4cb21047944f350e4a75e88914e1e1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 21:37:26 +0200 Subject: commander: require home position for MISSION, fallback to LOITER --- src/modules/commander/commander.cpp | 7 +++++++ src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..1da364aa5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1718,6 +1718,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } print_reject_mode(status, "AUTO_MISSION"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // fallback to POSCTL diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 423ce2f23..7fb2e08db 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { @@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { -- cgit v1.2.3 From 91537934c4ef33444cf582f8a309443d77b3e575 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 14:58:27 -0700 Subject: Better logging --- src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..b2e6e9d30 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -199,12 +199,18 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workList(Request *req) { auto hdr = req->header(); - DIR *dp = opendir(req->dataAsCString()); + + char dirPath[kMaxDataLength]; + strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + + DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", req->dataAsCString()); + printf("FTP: can't open path '%s'\n", dirPath); return kErrNotDir; } + + printf("FTP: list %s\n", dirPath); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -216,6 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { + printf("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -251,8 +258,8 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset]); offset += strlen(entry.d_name) + 1; - printf("FTP: list %s\n", entry.d_name); } closedir(dp); -- cgit v1.2.3 From 324322cb29720dd78b6eb534bb679532d5ed83f2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 02:10:09 +0400 Subject: UAVCAN ESC perf counters --- src/modules/uavcan/esc_controller.cpp | 32 ++++++++++++++++++++++++-------- src/modules/uavcan/esc_controller.hpp | 8 ++++++++ 2 files changed, 32 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index f603d9448..406eba88c 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -48,6 +48,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : { } +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + int UavcanEscController::init() { // ESC status subscription @@ -67,8 +73,10 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - assert(outputs != nullptr); - assert(num_outputs <= MAX_ESCS); + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } /* * Rate limiting - we don't want to congest the bus @@ -89,13 +97,21 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) + if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely - - assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); - assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); - - msg.cmd.push_back(scaled); + } + + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } + + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp index 0ed0c59b5..559ede561 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/esc_controller.hpp @@ -47,11 +47,13 @@ #include #include #include +#include class UavcanEscController { public: UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); int init(); @@ -96,4 +98,10 @@ private: */ bool _armed = false; uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); }; -- cgit v1.2.3 From b8ba6400f26e573236d60929283c673fb620f28e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:04:30 -0700 Subject: More logging --- src/modules/mavlink/mavlink_ftp.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b2e6e9d30..2da381a2d 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -210,7 +210,7 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - printf("FTP: list %s\n", dirPath); + printf("FTP: list %s offset %d\n", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -258,7 +258,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset]); + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); offset += strlen(entry.d_name) + 1; } -- cgit v1.2.3 From 2a79689224a381cf777c555d330f0a2aca8e3d9a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:51:06 -0700 Subject: Fix unused variable warnings --- src/modules/commander/commander.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 3 +++ 5 files changed, 8 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f83c1caf8..3d4a297f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -773,7 +773,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ 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 3e835cf81..3d6c62434 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 @@ -807,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -945,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Calculate altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..2e37d166e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..55a472bf6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 00c649c75..df3ddb966 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> %% Vector<3>", v1 % v2); TEST_OP("Vector<3> length", v1.length()); TEST_OP("Vector<3> length squared", v1.length_squared()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); +#pragma GCC diagnostic pop TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } -- cgit v1.2.3 From 72a531b018f0089c89ed40261969555fd282e459 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:07:37 +0400 Subject: Fixed UAVCAN GNSS bridge - EPV computation, catching up with the new GPS ORB topic --- src/modules/uavcan/gnss_receiver.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 3e98bdf14..debba9fee 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -85,13 +85,18 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.p_variance_m = -1.0; - _report.eph_m = -1.0; + _report.eph = -1.0F; + _report.epv = -1.0F; } if (valid_velocity_covariance) { @@ -118,12 +123,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); -- cgit v1.2.3 From 664795c9db9a0d938cbe7221aed87755ca8de7bf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:47:40 +0400 Subject: UAVCAN GNSS - using GNSS time to initialize the field time_gps_usec --- src/modules/uavcan/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index debba9fee..ba1fe5e49 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -139,7 +139,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Mon, 7 Jul 2014 15:13:27 +0200 Subject: navigator: forgot to fix conflicts --- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ae2e01c8c..bf6e2ea0e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -113,7 +113,7 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c959bf891..1a5ba4c1a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -124,7 +124,6 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), - _update_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { -- cgit v1.2.3 From 29bf1fe6fa40968f1cda53c3aa9f4dad3ec25ebb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Jul 2014 15:18:12 +0200 Subject: dataman: added macro for offboard storage selection --- src/modules/dataman/dataman.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dbace21ef..d625bf985 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -57,6 +57,8 @@ extern "C" { DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; + #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, -- cgit v1.2.3 From a29f7cad395ce53b74500a0dc03214186c679378 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Jul 2014 15:18:54 +0200 Subject: navigator: reject mission if the first waypoint is too far from home --- src/modules/navigator/mission.cpp | 69 +++++++++++++++++++++++++++++++++- src/modules/navigator/mission.h | 10 ++++- src/modules/navigator/mission_params.c | 13 ++++++- 3 files changed, 89 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 291e2edeb..53f0724cd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -61,6 +61,7 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), + _param_dist_1wp(this, "DIST_1WP"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -69,8 +70,10 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), + _dist_1wp_ok(false), _need_takeoff(true), _takeoff(false) + { /* load initial params */ updateParams(); @@ -246,6 +249,70 @@ Mission::advance_mission() } } +bool +Mission::check_dist_1wp() +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + bool mission_valid = false; + if (_param_dist_1wp.get() > 0.0f) { + if (_navigator->get_vstatus()->condition_home_position_valid) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (int i = 0; i < _offboard_mission.count; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from home to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < _param_dist_1wp.get()) { + _dist_1wp_ok = true; + return true; + + } else { + /* item is too far from home */ + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); + return false; + } + + } else { + return true; + } +} + void Mission::set_mission_items() { @@ -266,7 +333,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (read_mission_item(false, true, &_mission_item)) { + } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 40629b1b2..4da6a1155 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,6 +91,12 @@ private: */ void advance_mission(); + /** + * Check distance to first waypoint (with lat/lon) + * @return true only if it's not too far from home (< MIS_DIST_1WP) + */ + bool check_dist_1wp(); + /** * Set new mission items */ @@ -127,8 +133,9 @@ private: */ void publish_mission_result(); - control::BlockParamFloat _param_onboard_enabled; + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; + control::BlockParamFloat _param_dist_1wp; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -148,6 +155,7 @@ private: } _mission_type; bool _inited; + bool _dist_1wp_ok; MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8692328db..5cb3782c9 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,7 +58,6 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); - /** * Enable onboard mission * @@ -67,3 +66,15 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); + +/** + * Maximal horizontal distance from home to first waypoint + * + * Failsafe check to prevent running mission stored from previous flight on new place. + * Value < 0 means that check disabled. + * + * @min -1 + * @max 200 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); -- cgit v1.2.3 From 3b06a74074f71c0700c6e7e9b56954213142aaeb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Jul 2014 18:18:13 +0200 Subject: navigator: adapt offboard class to new NavigatorMode API --- src/modules/navigator/offboard.cpp | 27 ++++++++++++++++----------- src/modules/navigator/offboard.h | 16 +++------------- 2 files changed, 19 insertions(+), 24 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index ef7d11a03..dcb5c6000 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -67,17 +67,27 @@ Offboard::~Offboard() { } -bool -Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Offboard::on_inactive() +{ +} + +void +Offboard::on_activation() +{ +} + +void +Offboard::on_active() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + bool updated; orb_check(_navigator->get_offboard_control_sp_sub(), &updated); if (updated) { update_offboard_control_setpoint(); } - bool changed = false; - /* copy offboard setpoints to the corresponding topics */ if (_navigator->get_control_mode()->flag_control_position_enabled && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { @@ -91,7 +101,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.position_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } else if (_navigator->get_control_mode()->flag_control_velocity_enabled && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { @@ -105,16 +115,11 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.velocity_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } - return changed; } -void -Offboard::on_inactive() -{ -} void Offboard::update_offboard_control_setpoint() diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h index 4d415dddb..66b923bdb 100644 --- a/src/modules/navigator/offboard.h +++ b/src/modules/navigator/offboard.h @@ -54,25 +54,15 @@ class Navigator; class Offboard : public NavigatorMode { public: - /** - * Constructor - */ Offboard(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Offboard(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); + + virtual void on_active(); private: void update_offboard_control_setpoint(); -- cgit v1.2.3 From f5c14ff140804b351464e53ec264945f9bcd48f3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 10:17:39 -0700 Subject: Turn off noisier logging --- src/modules/mavlink/mavlink_ftp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 2da381a2d..e0a52f393 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -210,7 +210,7 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - printf("FTP: list %s offset %d\n", dirPath, hdr->offset); + printf("FTP: list %s offset %d\n", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -222,7 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - printf("FTP: list %s readdir_r failure\n", dirPath); + printf("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -258,7 +258,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); offset += strlen(entry.d_name) + 1; } -- cgit v1.2.3 From 680ebf29c3f8298e63d4c4d9c9076464e6f4b3c1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 15:11:46 -0700 Subject: Fix compiler warnings --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++++- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 +----- src/modules/navigator/mission.cpp | 8 ++++---- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/uORB/topics/telemetry_status.h | 10 ++++++++++ src/systemcmds/param/param.c | 1 - src/systemcmds/tests/test_mathlib.cpp | 2 ++ 7 files changed, 24 insertions(+), 13 deletions(-) (limited to 'src/modules') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 808b635bb..d8d45d34e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -139,7 +139,11 @@ ARCHWARNINGS = -Wall \ -Werror=format-security \ -Werror=array-bounds \ -Wfatal-errors \ - -Wformat=1 + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=unused-variable \ + -Werror=double-promotion \ + -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 973de1382..f33a1d780 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1742,7 +1742,6 @@ void AttPosEKF::FuseOptFlow() static float vd = 0.0f; static float pd = 0.0f; static float ptd = 0.0f; - static Vector3f delAng; static float R_LOS = 0.01f; static float losPred[2]; @@ -1820,9 +1819,6 @@ void AttPosEKF::FuseOptFlow() // calculate relative velocity in sensor frame relVelSensor = Tns*velNED_local; - // calculate delta angles in sensor axes - Vector3f delAngRel = Tbs*delAng; - // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; losPred[1] = -relVelSensor.x/range; @@ -1959,7 +1955,7 @@ void AttPosEKF::FuseOptFlow() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cf6a764bf..d39ca6cf9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,11 +65,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), + _need_takeoff(true), + _takeoff(false), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE), - _need_takeoff(true), - _takeoff(false) + _mission_type(MISSION_TYPE_NONE) { /* load initial params */ updateParams(); @@ -334,7 +334,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a1c6f09d0..4adf77dce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -57,10 +57,10 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), + _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item({0}) + _time_first_inside_orbit(0) { } diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index c4b99d520..3347724a5 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -86,4 +86,14 @@ static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_I ORB_ID(telemetry_status_3), }; +// This is a hack to quiet an unused-variable warning for when telemetry_status.h is +// included but telemetry_status_orb_id is not referenced. The inline works if you +// choose to use it, but you can continue to just directly index into the array as well. +// If you don't use the inline this ends up being a no-op with no additional code emitted. +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) +{ + return telemetry_status_orb_id[index]; +} + #endif /* TOPIC_TELEMETRY_STATUS_H */ diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 235ab7186..28e1b108b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -231,7 +231,6 @@ do_show_print(void *arg, param_t param) /* start search */ const char *ss = search_string; const char *pp = p_name; - bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ while (*ss != '\0' && *pp != '\0') { diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index df3ddb966..70d173fc9 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -98,6 +98,8 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> length squared", v1.length_squared()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); #pragma GCC diagnostic pop -- cgit v1.2.3 From 6f1292838960ca9eb72dc1818250eb50f70ddb98 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 20:00:44 -0700 Subject: Fix mainStateTransitionTest --- .../commander_tests/state_machine_helper_test.cpp | 213 ++++++++++++++------- 1 file changed, 147 insertions(+), 66 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index ee0d08391..2e18c4284 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Safety switch arming tests - { "transition: init to standby, no safety switch", + { "transition: standby to armed, no safety switch", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - { "transition: init to standby, safety switch off", + { "transition: standby to armed, safety switch off", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool StateMachineHelperTest::mainStateTransitionTest(void) { - struct vehicle_status_s current_state; - main_state_t new_main_state; + // This structure represent a single test case for testing Main State transitions. + typedef struct { + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + main_state_t from_state; // State prior to transition request + main_state_t to_state; // State to transition to + transition_result_t expected_transition_result; // Expected result from main_state_transition call + } MainTransitionTest_t; + + // Bits for condition_bits + #define MTT_ALL_NOT_VALID 0 + #define MTT_ROTARY_WING 1 << 0 + #define MTT_LOC_ALT_VALID 1 << 1 + #define MTT_LOC_POS_VALID 1 << 2 + #define MTT_HOME_POS_VALID 1 << 3 + #define MTT_GLOBAL_POS_VALID 1 << 4 + + static const MainTransitionTest_t rgMainTransitionTests[] = { + + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + { "transition: MANUAL to ACRO", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + + { "transition: ACRO to MANUAL", + MTT_ALL_NOT_VALID, + MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_MISSION - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + + { "transition: AUTO_MISSION to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_LOITER - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + + { "transition: AUTO_LOITER to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + + { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - not rotary", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", + MTT_ROTARY_WING | MTT_LOC_ALT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", + MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: ALTCTL to MANUAL - local altitude valid", + MTT_LOC_ALT_VALID, + MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position not valid, global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position valid, global position not valid", + MTT_LOC_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: POSCTL to MANUAL - local position valid, global position valid", + MTT_LOC_POS_VALID, + MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + { "no transition: MANUAL to AUTO_MISSION - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_LOITER - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", + MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", + MTT_ROTARY_WING, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + }; - // Identical states. - current_state.main_state = MAIN_STATE_MANUAL; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // AUTO to MANUAL. - current_state.main_state = MAIN_STATE_AUTO; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("transition changed: auto to manual", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to ALTCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("tranisition: manual to altctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - - // MANUAL to ALTCTRL, invalid local altitude. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("no transition: invalid local altitude", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to POSCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("transition: manual to posctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - - // MANUAL to POSCTRL, invalid local position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("no transition: invalid position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to AUTO. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = true; - new_main_state = MAIN_STATE_AUTO; - ut_assert("transition: manual to auto", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); - - // MANUAL to AUTO, invalid global position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = false; - new_main_state = MAIN_STATE_AUTO; - ut_assert("no transition: invalid global position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); + for (size_t i=0; ifrom_state; + current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; + current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; + current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; + current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; + current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; + + // Attempt transition + transition_result_t result = main_state_transition(¤t_state, test->to_state); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + if (test->expected_transition_result == result) { + if (test->expected_transition_result == TRANSITION_CHANGED) { + ut_assert(test->assertMsg, test->to_state == current_state.main_state); + } else { + ut_assert(test->assertMsg, test->from_state == current_state.main_state); + } + } + } + return true; } -- cgit v1.2.3 From 0d0c4c36265b044ba978cfe4dbe369e93aa25d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:57:25 +0200 Subject: mc pos control: Fix reordering warnings --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4e88e260c..9b0f69226 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -106,31 +106,31 @@ public: private: - bool _task_should_exit; /**< if true, task should exit */ + bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _local_pos_sp_sub; /**< offboard local position setpoint */ - int _global_vel_sp_sub; /**< offboard global velocity setpoint */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ @@ -258,8 +258,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _global_vel_sp_sub(-1), _pos_sp_triplet_sub(-1), + _global_vel_sp_sub(-1), /* publications */ _att_sp_pub(-1), -- cgit v1.2.3 From 9f1661f1f8808b7c83f73284cee9a1e9279165d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:58:08 +0200 Subject: Silence FTP transactions, be vocal about errors --- src/modules/mavlink/mavlink_ftp.cpp | 19 ++++++++++--------- src/modules/mavlink/mavlink_ftp.h | 27 ++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 25 insertions(+), 23 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 57ae6076c..675a6870e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -117,10 +117,10 @@ MavlinkFTP::_worker(Request *req) if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; - printf("ftp: bad crc\n"); + warnx("ftp: bad crc"); } - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -167,9 +167,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - printf("FTP: ack\n"); + //warnx("FTP: ack\n"); } else { - printf("FTP: nak %u\n", errorCode); + warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; hdr->size = 1; hdr->data[0] = errorCode; @@ -206,11 +206,11 @@ MavlinkFTP::_workList(Request *req) DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", dirPath); + warnx("FTP: can't open path '%s'", dirPath); return kErrNotDir; } - printf("FTP: list %s offset %d\n", dirPath, hdr->offset); + //warnx("FTP: list %s offset %d", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -222,7 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - printf("FTP: list %s readdir_r failure\n", dirPath); + warnx("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -304,19 +304,20 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - printf("Seek %d\n", hdr->offset); + //warnx("seek %d", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location + warnx("seek fail"); return kErrEOF; } int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof + warnx("read fail %d", bytes_read); return kErrIO; } - printf("Read success %d\n", bytes_read); hdr->size = bytes_read; return kErrNone; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index e22e61553..59237a4c4 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -147,20 +147,21 @@ private: unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), _mavlink->get_channel(), &msg, sequence(), rawData()); - _mavlink->lockMessageBufferMutex(); - bool fError = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!fError) { + _mavlink->lockMessageBufferMutex(); + bool success = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!success) { warnx("FTP TX ERR"); - } else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } + } + // else { + // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + // _mavlink->get_system_id(), + // _mavlink->get_component_id(), + // _mavlink->get_channel(), + // len, + // msg.checksum); + // } } uint8_t *rawData() { return &_message.data[0]; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81966caf8..484886682 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -442,7 +442,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", manual.x, manual.y, manual.r, manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); -- cgit v1.2.3 From 37b4cdfce21ff6a7374599a6706ad387bd359515 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Mar 2014 10:56:33 +1100 Subject: board_serial: use a uint8_t buffer we should not be using 'char' for binary APIs, as the C standard does not specify if it is signed or unsigned, so results may not be consistent --- src/drivers/px4fmu/fmu.cpp | 2 +- src/modules/systemlib/board_serial.c | 8 ++++---- src/modules/systemlib/board_serial.h | 2 +- src/modules/systemlib/otp.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728..39cc32967 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "id")) { - char id[12]; + uint8_t id[12]; (void)get_board_serial(id); errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c index ad8c2bf83..182fd15c6 100644 --- a/src/modules/systemlib/board_serial.c +++ b/src/modules/systemlib/board_serial.c @@ -44,11 +44,11 @@ #include "board_config.h" #include "board_serial.h" -int get_board_serial(char *serialid) +int get_board_serial(uint8_t *serialid) { - const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START; union udid id; - val_read((unsigned *)&id, udid_ptr, sizeof(id)); + val_read((uint32_t *)&id, udid_ptr, sizeof(id)); /* Copy the serial from the chips non-write memory and swap endianess */ @@ -57,4 +57,4 @@ int get_board_serial(char *serialid) serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; return 0; -} \ No newline at end of file +} diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h index b14bb4376..873d9657b 100644 --- a/src/modules/systemlib/board_serial.h +++ b/src/modules/systemlib/board_serial.h @@ -44,6 +44,6 @@ __BEGIN_DECLS -__EXPORT int get_board_serial(char *serialid); +__EXPORT int get_board_serial(uint8_t *serialid); __END_DECLS diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index f10e129d8..273b064f0 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -125,7 +125,7 @@ struct otp_lock { #pragma pack(push, 1) union udid { uint32_t serial[3]; - char data[12]; + uint8_t data[12]; }; #pragma pack(pop) -- cgit v1.2.3 From 9b2d444cc56eaaedcf271f200b93dcca94623209 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 14:08:09 +0200 Subject: dataman: use DM_KEY_WAYPOINTS_OFFBOARD() macro everywhere --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- src/modules/navigator/mission.cpp | 23 +++++------------------ 2 files changed, 7 insertions(+), 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b2288469c..0dda2337e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -216,7 +216,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); struct mission_item_s mission_item; if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { @@ -650,7 +650,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) return; } - dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 53f0724cd..06e915f27 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -201,19 +201,11 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, - (size_t)_offboard_mission.count, - _navigator->get_geofence(), - _navigator->get_home_position()->alt); + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); @@ -474,12 +466,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s mission = &_offboard_mission; - if (_offboard_mission.dataman_id == 0) { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; - } + dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { -- cgit v1.2.3 From 3157b06fee99b57fe336f7772b293f8689ff8cdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 15:52:19 +0200 Subject: Generalized the airspeed check --- src/modules/commander/state_machine_helper.cpp | 29 ++++++++++---------------- 1 file changed, 11 insertions(+), 18 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..d894c9db0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || + hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } -- cgit v1.2.3 From e57579fa21ce9be189475e41fbcdb961d7cd32d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 17:08:23 +0200 Subject: mavlink, navigator: compile errors/warnings fixed --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_mission.cpp | 2 +- src/modules/navigator/mission.cpp | 12 ++++-------- 3 files changed, 7 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c7eefa98..0d685c3d8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -226,8 +226,9 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), - _mission_result_sub(-1), + _mission_manager(nullptr), _mission_pub(-1), + _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), _verbose(false), @@ -245,7 +246,6 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), - _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0dda2337e..ca01344d4 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -69,9 +69,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _dataman_id(0), _count(0), _current_seq(0), - _dataman_id(0), _transfer_dataman_id(0), _transfer_count(0), _transfer_seq(0), diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eb8dcc717..126877651 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,10 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false), - _need_takeoff(true), - _takeoff(false) - + _dist_1wp_ok(false) { /* load initial params */ updateParams(); @@ -203,7 +200,7 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), @@ -252,13 +249,12 @@ Mission::check_dist_1wp() } /* check if first waypoint is not too far from home */ - bool mission_valid = false; if (_param_dist_1wp.get() > 0.0f) { if (_navigator->get_vstatus()->condition_home_position_valid) { struct mission_item_s mission_item; /* find first waypoint (with lat/lon) item in datamanager */ - for (int i = 0; i < _offboard_mission.count; i++) { + for (unsigned i = 0; i < _offboard_mission.count; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { @@ -281,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); return false; } } -- cgit v1.2.3 From 687612dd654bcdfb390bfdddf8a004b10119c2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 18:34:33 +0200 Subject: Do not abort if the sensor reset failed, only abort on no data --- src/modules/commander/airspeed_calibration.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d0..923c1a3ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -95,10 +95,8 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + warn("FAILED to reset - assuming analog"); + mavlink_log_critical(mavlink_fd, "assuming analog sensor"); } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From dca8daeec54624001f02acb7ad6f4c2d7304a5fb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 21:37:00 +0200 Subject: mavlink: use all outputs in HIL mode --- src/modules/mavlink/mavlink_messages.cpp | 55 +++++++++++++------------------- 1 file changed, 23 insertions(+), 32 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 667be87b7..c7ad605c5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1051,10 +1051,16 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ + /* multirotors: set number of rotor outputs depending on type */ + unsigned n; switch (mavlink_system.type) { @@ -1071,59 +1077,44 @@ protected: break; } - /* scale / assign outputs depending on system type */ - float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } } else { - out[i] = -1.0f; + /* send 0 when disarmed */ + out[i] = 0.0f; } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + /* scale PWM out 900..2100 us to -1..1 for normal channels */ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + /* scale PWM out 900..2100 us to 0..1 for throttle */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } }; -- cgit v1.2.3 From 3461d3d215843681b1537256d26b053e3f2b78e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:35 +0200 Subject: Introduce analog scaling parameter for analog airspeed sensor --- src/modules/sensors/sensor_params.c | 10 ++++++++-- src/modules/sensors/sensors.cpp | 18 +++++++++--------- 2 files changed, 17 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5deb471d6..6b90a1239 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -194,16 +194,22 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); /** * Differential pressure sensor offset * + * The offset (zero-reading) in Pascal + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); /** - * Differential pressure sensor analog enabled + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e6..0cad0c0e5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -248,7 +248,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; - float diff_pres_analog_enabled; + float diff_pres_analog_scale; int board_rotation; int external_mag_rotation; @@ -311,7 +311,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; - param_t diff_pres_analog_enabled; + param_t diff_pres_analog_scale; param_t rc_map_roll; param_t rc_map_pitch; @@ -590,7 +590,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); @@ -798,7 +798,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1323,22 +1323,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { - float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; - _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { -- cgit v1.2.3 From 8d081a8b0dd002cda075ee9d8e087fdf54722ccf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:11:03 +0200 Subject: uORB: Remove voltage field from airspeed --- src/modules/uORB/topics/differential_pressure.h | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 01e14cda9..cd48d3cb2 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,6 @@ struct differential_pressure_s { float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From 25f3f6e7f2dd87a831d25a9348a67ed918407d96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:12:31 +0200 Subject: airspeed calibration improvements for analog sensors --- src/modules/commander/airspeed_calibration.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 923c1a3ff..12e527a68 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -76,7 +76,7 @@ int do_airspeed_calibration(int mavlink_fd) /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -97,6 +97,12 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From 4c13c67504fb2b66c97ac6b902e67a864f1173e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:02:22 +0200 Subject: Hotfix: Close fd before reusing it again --- src/modules/commander/state_machine_helper.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..ca95f139e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -666,6 +666,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ + close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { -- cgit v1.2.3 From b97c867420f477f2c3a7dbc073975f5d872194cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:51:13 +0200 Subject: Add a check command and fix error reporting --- src/modules/commander/commander.cpp | 19 ++++++++++++------- src/modules/commander/state_machine_helper.cpp | 23 ++++++++++------------- src/modules/commander/state_machine_helper.h | 2 ++ 3 files changed, 24 insertions(+), 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2058cd92e..106f05f57 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -286,15 +286,20 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } - } else { - warnx("\tcommander not started"); - } + if (!strcmp(argv[1], "status")) { + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int checkres = prearm_check(&status, mavlink_fd); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca95f139e..d6b4d43ad 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,8 +70,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret != OK) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -646,22 +646,20 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) /* evaluate values */ float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.78f || accel_scale > 9.83f) { + if (accel_scale < 9.75f || accel_scale > 9.85f) { mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); } if (accel_scale > 30.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } @@ -672,7 +670,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -681,20 +679,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = read(fd, &diff_pres, sizeof(diff_pres)); if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } } system_eval: close(fd); - return ret; + return (!failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403e..ddc5bf154 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From a82d4fbb115e949a6a5c12d46308df1c20abfd99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:55:46 +0200 Subject: Hotfix: fix typo --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 106f05f57..6138a4c37 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -308,7 +308,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } -- cgit v1.2.3 From 79c5d434bd5911d11e6968e0a339b40a6f82e033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:10:11 +0200 Subject: Commander: More hotfixes to prearm check routine --- src/modules/commander/commander.cpp | 5 ++++- src/modules/commander/state_machine_helper.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6138a4c37..18761665a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -298,7 +298,9 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int checkres = prearm_check(&status, mavlink_fd); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -329,6 +331,7 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d6b4d43ad..5cbc95920 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -664,11 +664,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + if (fd <= 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; @@ -693,5 +694,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) system_eval: close(fd); - return (!failed); + return (failed); } -- cgit v1.2.3 From a118e8dbdd72e23a47841577dfcd7a45007eaedc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:17:21 +0200 Subject: Make commander error message more verbose --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5cbc95920..ce1bd9ada 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -647,7 +647,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); } if (accel_scale > 30.0f /* m/s^2 */) { -- cgit v1.2.3 From 11d9724563f005ed722d326e8bef125bfec29865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 16:03:27 +0200 Subject: Stop warning users just because they shake a bit, be more strict to catch sensor failures or calibration errors in time --- src/modules/commander/state_machine_helper.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ce1bd9ada..363f5e8eb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -644,14 +644,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); - } - - if (accel_scale > 30.0f /* m/s^2 */) { + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 899657613e44f0d4bbdb66644b35420d7381ff99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:25 +0200 Subject: airspeed cal: Improve user feedback --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 12e527a68..7599fc777 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -96,7 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); -- cgit v1.2.3 From e3afb669cae54b042f453605601fed861c52ee45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:47 +0200 Subject: sensors: Fix usage of offset for measurements --- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6b90a1239..8f6e7abe6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * * Pick the appropriate scaling from the datasheet. * this number defines the (linear) conversion from voltage - * to Pascal (pa). + * to Pascal (pa). For the MPXV7002DP this is 1000. * * @group Sensor Calibration */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0cad0c0e5..cda79693d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1330,12 +1330,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; -- cgit v1.2.3 From 9c63aba9a72083afe1e5f818c6559760c5b6b1cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:44:54 +0200 Subject: Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS --- .../ekf_att_pos_estimator_main.cpp | 26 +++++++++++++--------- 1 file changed, 15 insertions(+), 11 deletions(-) (limited to 'src/modules') 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 5d768b73d..bc5f709e5 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 @@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // Guard before running any parts of the filter + // of NaN / invalid values + if (_ekf->statesInitialised) { + + // We're apparently initialized in this case now + + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized && _ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; -- cgit v1.2.3 From 2fa4434a03abe057e65a5c8ba29f8cd9cd75ca93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 00:17:50 +0200 Subject: Limit output of driver to positive range --- src/modules/sensors/sensors.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cda79693d..1ffd7f02f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -501,6 +501,7 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1332,12 +1333,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa; - _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ -- cgit v1.2.3 From 065bf013a407da5e2a70db82013126c20e2ad429 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:30 +0200 Subject: More obvious error message for mission reject --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 126877651..ba766cd10 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -277,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); + mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); return false; } } -- cgit v1.2.3 From 5616a07bf306b3fa2bc70078e9c8c26a086065dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:56 +0200 Subject: Final default values and more comments for params --- src/modules/navigator/mission_params.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 5cb3782c9..881caa24e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -59,22 +59,26 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); /** - * Enable onboard mission + * Enable persistent onboard mission storage + * + * When enabled, missions that have been uploaded by the GCS are stored + * and reloaded after reboot persistently. * * @min 0 * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); /** * Maximal horizontal distance from home to first waypoint * - * Failsafe check to prevent running mission stored from previous flight on new place. - * Value < 0 means that check disabled. + * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. + * Set a value of zero or less to disable. The mission will not be started if the current + * waypoint is more distant than MIS_DIS_1WP from the current position. * - * @min -1 - * @max 200 + * @min 0 + * @max 250 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); -- cgit v1.2.3 From 629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:30:49 +0200 Subject: Fix severity handling of text messages --- src/modules/mavlink/mavlink_main.cpp | 39 ++++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 46 ++++++++++++++++++++++++++++-------- 2 files changed, 68 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d685c3d8..cd0581af4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -492,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); + msg.severity = (unsigned char)cmd; Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -840,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - send_statustext("[mavlink pm] sending list"); + send_statustext_info("[pm] sending list"); } } break; @@ -864,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - send_statustext(buf); + send_statustext_info(buf); } else { /* set and send parameter */ @@ -901,17 +902,28 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } int -Mavlink::send_statustext(const char *string) +Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAV_SEVERITY_INFO, string); + return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } int -Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) +Mavlink::send_statustext_critical(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); +} + +int +Mavlink::send_statustext_emergency(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); +} + +int +Mavlink::send_statustext(unsigned severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = severity; int i = 0; @@ -929,6 +941,19 @@ Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) /* Enforce null termination */ statustext.text[i] = '\0'; + /* Map severity */ + switch (severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; + } + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; @@ -1449,7 +1474,7 @@ Mavlink::task_main(int argc, char *argv[]) int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - send_statustext(msg.text); + send_statustext(msg.severity, msg.text); } } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f4fb759f..acfc8b90e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,9 +157,9 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + void send_message(const mavlink_message_t *msg); - void handle_message(const mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -174,17 +174,43 @@ public: mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - int send_statustext(const char *string); - int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() const { return _streams; } + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level, one of + */ + int send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } - float get_rate_mult(); + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -195,13 +221,13 @@ public: bool message_buffer_write(const void *ptr, int size); - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error */ - void count_txerr(); + void count_txerr(); protected: Mavlink *next; -- cgit v1.2.3 From 7b768d11c3c8d57807495bf2abb324bb5f14aa14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:31:09 +0200 Subject: Improve mission feedback, fix compile errors --- src/modules/mavlink/mavlink_mission.cpp | 58 +++++++++++++++++++-------------- src/modules/mavlink/mavlink_mission.h | 36 ++++++++++---------- 2 files changed, 52 insertions(+), 42 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index ca01344d4..a62d34019 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -189,7 +189,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) } else { if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } - _mavlink->send_statustext("ERROR: wp index out of bounds"); + _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); } } @@ -238,7 +238,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to read from micro SD"); + _mavlink->send_statustext_critical("Unable to read from micro SD"); if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } @@ -262,7 +262,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { - _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } } @@ -314,7 +314,7 @@ MavlinkMissionManager::eventloop() /* check for timed-out operations */ if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { - _mavlink->send_statustext("Operation timeout"); + _mavlink->send_statustext_critical("Operation timeout"); if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } @@ -390,6 +390,8 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } } else { + _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE"); + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } } @@ -397,7 +399,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } } @@ -421,18 +423,20 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } - _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); } } } @@ -459,6 +463,8 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + + _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); @@ -466,7 +472,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); } } } @@ -489,7 +495,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _transfer_seq++; - } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + } else if (wpr.seq == _transfer_seq - 1) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } } else { @@ -506,36 +512,36 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); return; } /* double check bounds in case of items count changed */ - if (wpr.seq >= 0 && wpr.seq < _count) { + if (wpr.seq < _count) { send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); } _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); } } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer"); } else { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } @@ -565,7 +571,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); // TODO send ACK? return; @@ -588,19 +594,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* looks like our MISSION_REQUEST was lost, try again */ if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } - _mavlink->send_statustext("WP CMD OK AGAIN"); + _mavlink->send_statustext_info("WP CMD OK TRY AGAIN"); } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); return; } } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); return; } @@ -629,13 +635,13 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } @@ -645,6 +651,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; return; @@ -656,7 +664,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; return; } @@ -674,6 +682,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + _mavlink->send_statustext_info("WPM: Transfer complete."); + _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { @@ -712,7 +722,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index d8695cf83..1964a45e8 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -142,35 +142,35 @@ public: void set_verbose(bool v) { _verbose = v; } private: - Mavlink* _mavlink; + Mavlink* _mavlink; mavlink_channel_t _channel; - uint8_t _comp_id; + uint8_t _comp_id; - enum MAVLINK_WPM_STATES _state; ///< Current state + enum MAVLINK_WPM_STATES _state; ///< Current state - uint64_t _time_last_recv; - uint64_t _time_last_sent; + uint64_t _time_last_recv; + uint64_t _time_last_sent; - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - int _transfer_seq; ///< Item sequence in current transmission - int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission int _offboard_mission_sub; int _mission_result_sub; - orb_advert_t _offboard_mission_pub; + orb_advert_t _offboard_mission_pub; - MavlinkRateLimiter _slow_rate_limiter; + MavlinkRateLimiter _slow_rate_limiter; bool _verbose; }; -- cgit v1.2.3 From e64a28e736224da5d1db8e3477eeeffc0b3b1f6c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 12 Jul 2014 17:34:36 +0400 Subject: Building UAVCAN without run-time checks. This saves 9.5KB of flash and reduces CPU usage. --- src/modules/uavcan/module.mk | 10 +++++----- uavcan | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2c75944d4..1ef6f0cfa 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp \ gnss_receiver.cpp # @@ -53,7 +53,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS # # libuavcan drivers for STM32 @@ -68,7 +68,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # TODO: Add make target for this, or invoke dsdlc manually. # The second option assumes that the generated headers shall be saved # under the version control, which may be undesirable. -# The first option requires python3 and python3-mako for the sources to be built. +# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated diff --git a/uavcan b/uavcan index f66c1a7de..af065e9ca 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f66c1a7de3076ff956bdf159dc3a166cbffe6089 +Subproject commit af065e9ca9e3ba9100c125d3ab739313d0500ca8 -- cgit v1.2.3 From a0d150332ab67dbc501b10fa8adb97d91a79f23c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 15:39:04 +0200 Subject: Add note to param about tubes --- src/modules/sensors/sensor_params.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 8f6e7abe6..38b190761 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -207,6 +207,9 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * this number defines the (linear) conversion from voltage * to Pascal (pa). For the MPXV7002DP this is 1000. * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); -- cgit v1.2.3 From 45617e9f4385e80846c571b237e220216192d6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:14 +0200 Subject: Airspeed calibration improvement, enforce correct pitot order --- src/modules/commander/airspeed_calibration.cpp | 137 ++++++++++++++++++++----- src/modules/ekf_att_pos_estimator/InertialNav | 1 + 2 files changed, 112 insertions(+), 26 deletions(-) create mode 160000 src/modules/ekf_att_pos_estimator/InertialNav (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 7599fc777..a1a20abc9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -64,14 +64,12 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ @@ -105,29 +103,58 @@ int do_airspeed_calibration(int mavlink_fd) } } - while (calibration_counter < calibration_count) { + for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + unsigned calibration_counter = 0; - int poll_ret = poll(fds, 1, 1000); + if (i == 0) { + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + float diff_pres_offset = 0.0f; - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); - } + while (calibration_counter < calibration_count) { - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - close(diff_pres_sub); - return ERROR; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + if (i > 0) { + + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } } } @@ -151,14 +178,72 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + diff_pres_offset = 0.0f; + + /* just take a few samples and make sure pitot tubes are not reversed */ + while (calibration_counter < 50) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + float curr_avg = (diff_pres_offset / calibration_counter); + + if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if (curr_avg < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + + diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + (void)param_save_default(); + + return ERROR; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav new file mode 160000 index 000000000..8b65d755b --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/InertialNav @@ -0,0 +1 @@ +Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 -- cgit v1.2.3 From ffebe45c4ce6cb248314141d91abcb74fbf9174e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:25 +0200 Subject: mavlink: Handle unhandled enum cases --- src/modules/mavlink/mavlink_messages.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c7ad605c5..7c864f127 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; + } *mavlink_custom_mode = custom_mode.data; -- cgit v1.2.3 From 0144a7dacfdb398c6133a9bf2df35facdbdfe6e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:43:09 +0200 Subject: mavlink: Optimize to native types where possible, move things to optimize alignment --- src/modules/mavlink/mavlink_mission.cpp | 6 +++--- src/modules/mavlink/mavlink_mission.h | 11 ++++++----- 2 files changed, 9 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a62d34019..068774c47 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -61,8 +61,6 @@ static const int ERROR = -1; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink), - _channel(mavlink->get_channel()), - _comp_id(MAV_COMP_ID_MISSIONPLANNER), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -82,7 +80,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), - _verbose(false) + _verbose(false), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 1964a45e8..f63c32f24 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -143,8 +143,6 @@ public: private: Mavlink* _mavlink; - mavlink_channel_t _channel; - uint8_t _comp_id; enum MAVLINK_WPM_STATES _state; ///< Current state @@ -153,7 +151,7 @@ private: uint32_t _action_timeout; uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + unsigned _max_count; ///< Maximum number of mission items int _dataman_id; ///< Dataman storage ID for active mission unsigned _count; ///< Count of items in active mission @@ -163,8 +161,8 @@ private: unsigned _transfer_count; ///< Items count in current transmission unsigned _transfer_seq; ///< Item sequence in current transmission unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission int _offboard_mission_sub; int _mission_result_sub; @@ -173,4 +171,7 @@ private: MavlinkRateLimiter _slow_rate_limiter; bool _verbose; + + mavlink_channel_t _channel; + uint8_t _comp_id; }; -- cgit v1.2.3 From 0d1ac4235411e8f05f96bcbe51558d92f0d86cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:10:08 +0200 Subject: airspeed calibration: Update and resolve compile errors --- src/modules/commander/airspeed_calibration.cpp | 75 ++++++++++++-------------- 1 file changed, 33 insertions(+), 42 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1a20abc9..421e53ae0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd) } } - for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { + unsigned calibration_counter = 0; - unsigned calibration_counter = 0; - - if (i == 0) { - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); - usleep(500 * 1000); - } - - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); - if (i > 0) { + while (calibration_counter < calibration_count) { - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - } + int poll_ret = poll(fds, 1, 1000); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); - } + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); return ERROR; } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } } -- cgit v1.2.3 From 8a6e69ed671d3de6b267518f1cd425b24e48c71e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:08:36 +0200 Subject: Fix up RAM usage of fixed wing apps --- src/modules/fw_att_control/module.mk | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 4 +++- 3 files changed, 6 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 1e600757e..89c6494c5 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control SRCS = fw_att_control_main.cpp \ fw_att_control_params.c + +MODULE_STACKSIZE = 1200 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 3d6c62434..98ccd09a5 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 @@ -1445,7 +1445,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3500, + 2000, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index af155fe08..15b575b50 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 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 @@ -43,3 +43,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs.cpp \ mtecs/limitoverride.cpp \ mtecs/mTecs_params.c + +MODULE_STACKSIZE = 1200 -- cgit v1.2.3 From f32a51f51597bb66c7e404a7dc8e723c32f44743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:14:01 +0200 Subject: Fixed battery ignore voltage to a higher value --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e6..c40e52a0d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -133,7 +133,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 3.5f +#define BATT_V_IGNORE_THRESHOLD 4.8f /** * HACK - true temperature is much less than indicated temperature in baro, -- cgit v1.2.3 From 7bf0e6f3e26ee9e10073293677563f6862758557 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:47:26 +0200 Subject: Better airspeed feedback --- src/modules/commander/airspeed_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 421e53ae0..a1b8816ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,13 +200,13 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < calc_indicated_airspeed(-5.0f)) { + if (curr_avg < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 67d23253c3b3ec04bfd9d5f7e09d3d405ceba7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:00:50 +0200 Subject: airspeed cal build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1b8816ff..529a5b32f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,7 +200,7 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } -- cgit v1.2.3 From 4824a4e8ac93d992e1401f768291cca65ba46acc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:18:59 +0200 Subject: Fix for dynamic / static part of calibration --- src/modules/commander/airspeed_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 529a5b32f..01db34be2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -121,19 +121,6 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -197,16 +184,16 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - float curr_avg = (diff_pres_offset / calibration_counter); + float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + if (fabsf(curr_avg) < 10.0f) { mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < -calc_indicated_airspeed(5.0f)) { + if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 34abf5c40cda538f204d313cc49715b5a938d168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:50:56 +0200 Subject: airspeed cal: Fix up logic --- src/modules/commander/airspeed_calibration.cpp | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be2..47c9c7af5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { -- cgit v1.2.3 From 65409ad2c8ba10d95cbcfa088951abc40eb46774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:00 +0200 Subject: Airspeed calibration improvements --- src/modules/commander/airspeed_calibration.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 47c9c7af5..d52cafb8f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -125,7 +125,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -165,6 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed */ while (calibration_counter < 10) { @@ -178,16 +180,18 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + + if (fabsf(calibrated_pa) < 9.0f) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", + (double)calibrated_pa); usleep(3000 * 1000); continue; } /* do not allow negative values */ - if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + if (calibrated_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -205,8 +209,9 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - close(diff_pres_sub); - return OK; + mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + break; } } else if (poll_ret == 0) { @@ -217,6 +222,8 @@ int do_airspeed_calibration(int mavlink_fd) } } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); -- cgit v1.2.3 From 1b6ad3e1990bec84489f949b195adba4412e1c7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:37:10 +0200 Subject: ekf: Logic cleanup and print code cleanup --- .../ekf_att_pos_estimator_main.cpp | 137 ++++++++------------- 1 file changed, 49 insertions(+), 88 deletions(-) (limited to 'src/modules') 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 bc5f709e5..5ce6bdb0a 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 @@ -559,61 +559,26 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "att pos estimator: "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "GPS velocity divergence"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; + const char* const feedback[] = { 0, + "NaN in states, resetting", + "stale IMU data, resetting", + "got initial position lock", + "excessive gyro offsets", + "GPS velocity divergence", + "excessive covariances", + "unknown condition"}; + + // Print out error condition + if (check) { + unsigned warn_index = static_cast(check); + unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); + + if (max_warn_index < warn_index) { + warn_index = max_warn_index; } - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); } struct estimator_status_report rep; @@ -1140,20 +1105,6 @@ FixedwingEstimator::task_main() float initVelNED[3]; - // Guard before running any parts of the filter - // of NaN / invalid values - if (_ekf->statesInitialised) { - - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - } - /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1216,8 +1167,17 @@ FixedwingEstimator::task_main() _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } else if (_ekf->statesInitialised) { + // We're apparently initialized in this case now + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1285,7 +1245,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (!_gps_initialized && _ekf->statesInitialised) { + } else if (!_gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; @@ -1307,7 +1267,7 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newHgtData && _ekf->statesInitialised) { + if (newHgtData) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; @@ -1321,7 +1281,7 @@ FixedwingEstimator::task_main() } // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { + if (newDataMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data @@ -1335,7 +1295,7 @@ FixedwingEstimator::task_main() } // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + if (newAdsData && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); @@ -1403,7 +1363,7 @@ FixedwingEstimator::task_main() _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, @@ -1494,27 +1454,28 @@ FixedwingEstimator::task_main() } - } - - } + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } + } + } } -- cgit v1.2.3 From 716e20fab6d63f3dc6f80c70f382d6b0f45f9340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:39:13 +0200 Subject: Fix accel cal docs. --- src/modules/commander/accelerometer_calibration.cpp | 3 +-- src/modules/commander/accelerometer_calibration.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab76..7a4e7a766 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c0977..6b7e16d44 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 -- cgit v1.2.3 From 259014b0d50426b0fbed3b9eac5a1d34aaa5b211 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:40:09 +0200 Subject: Documentation only and unused defines only cleanup. --- src/modules/commander/airspeed_calibration.h | 2 +- src/modules/commander/baro_calibration.cpp | 2 +- src/modules/commander/baro_calibration.h | 2 +- src/modules/commander/calibration_messages.h | 3 +-- src/modules/commander/calibration_routines.cpp | 3 +-- src/modules/commander/calibration_routines.h | 3 +-- src/modules/commander/mag_calibration.h | 2 +- src/modules/commander/state_machine_helper.h | 10 ++++------ 8 files changed, 11 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2..8c6b65ff1 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c4087..da98644b4 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -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 diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cf..ce78ae700 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.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 diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564d..b1e209efc 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7..5796204bf 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafd..3c8e49ee9 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1..c14f948cc 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ddc5bf154..bb1b87e71 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 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,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include #include #include -- cgit v1.2.3 From ea11bc6c4bf7180ae4e321eebcaf817eebb18e6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:41:39 +0200 Subject: Command handling: Fix up local variable usage and status prints. --- src/modules/commander/commander.cpp | 67 +++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 29 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e14dff197..009e0ac01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //XXX: to enable the parachute, a param needs to be set //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { + if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; -- cgit v1.2.3 From 3ec9ffa66166b10101887cc4077d1b02d4a0897b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:42:02 +0200 Subject: Buzzer and led: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 009e0ac01..85c9ccbaa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -697,11 +697,11 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); -- cgit v1.2.3 From 76f7960d77046c33a771be49b57e32c957e7a2ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:43:56 +0200 Subject: Mavlink text feedback: Remove now unneeded audio tag for critical messages, make overall printing more efficient. Remove buffers where no buffers are needed. --- src/modules/commander/commander.cpp | 55 +++++++++++++++++-------------------- 1 file changed, 25 insertions(+), 30 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 85c9ccbaa..76cfd5feb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,8 +775,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); } else { - warnx("reading mission state failed"); - mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -789,8 +790,6 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } - mavlink_log_info(mavlink_fd, "[cmd] started"); - int ret; pthread_attr_t commander_low_prio_attr; @@ -1083,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1187,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1270,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { @@ -1339,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1385,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1405,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + mavlink_log_critical(mavlink_fd, "arming state transition denied"); tune_negative(true); } @@ -1428,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1445,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } have_link = true; } else { if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1467,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -2010,15 +2009,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -2033,9 +2030,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -2048,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -2064,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From f2b30be92ae946e8e40faade6cb10edfb037fdaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:44:27 +0200 Subject: commander status leds: Fix -Wshadow warning. --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 76cfd5feb..e5ea237fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1650,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1676,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { - if (status->condition_local_position_valid) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1714,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } -- cgit v1.2.3 From 66d5bc20c095a4dbd937b21c4f5fc1a67205f2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:02 +0200 Subject: commander RC handling: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 42 ++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 21 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5ea237fc..0c4d48dea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1727,16 +1727,16 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode(status, "OFFBOARD"); + print_reject_mode(status_local, "OFFBOARD"); } else { return res; @@ -1751,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1830,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From afc8908d38ff2eaabc2ca35363ac04697152cb6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:32 +0200 Subject: commander: More docs-only changes in headers. --- src/modules/commander/commander_helper.cpp | 10 ++++++---- src/modules/commander/commander_helper.h | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f6..d5fe122cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2013, 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 @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * */ #include diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f..a49c9e263 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 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,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef COMMANDER_HELPER_H_ @@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity * @return the estimated remaining capacity in 0..1 */ float battery_remaining_estimate_voltage(float voltage, float discharged); -- cgit v1.2.3 From 7525c290f2cdfd058d54e5e6eb2d2e522d070889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:46:36 +0200 Subject: commander: more text feedback improvements. --- src/modules/commander/state_machine_helper.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 363f5e8eb..87a60e7fd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); failed = true; goto system_eval; } @@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); failed = true; goto system_eval; } @@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); - mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); + mavlink_log_critical(mavlink_fd, "hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ failed = true; goto system_eval; @@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd <= 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; } @@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(diff_pres)) { if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 0d51230d4e3b87dd67819e594895b24a0a8a0306 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:04:43 +0200 Subject: commander: Final MAVLink text feedback cleanup --- src/modules/commander/state_machine_helper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87a60e7fd..56c59ad3d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From a32577377b8d735c77ffaaee15e2bbfa74be703f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:38:43 +0200 Subject: EKF init improvements --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 ++++++++ src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 16 +++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) (limited to 'src/modules') 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 5ce6bdb0a..ee1561280 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 @@ -610,6 +610,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main() _ekf->FuseVelposNED(); } else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d780..e83c37bbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) current_ekf_state.statesNaN = false; current_ekf_state.velHealth = true; - //current_ekf_state.posHealth = ?; - //current_ekf_state.hgtHealth = ?; + current_ekf_state.posHealth = true; + current_ekf_state.hgtHealth = true; current_ekf_state.velTimeout = false; - //current_ekf_state.posTimeout = ?; - //current_ekf_state.hgtTimeout = ?; + current_ekf_state.posTimeout = false; + current_ekf_state.hgtTimeout = false; + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; // Fill variables with valid data velNED[0] = initvelNED[0]; -- cgit v1.2.3 From 744eed91dca8a5f2ad8ead7ffa812d25755bcc93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:43:10 +0200 Subject: Fix calibration counter usage --- src/modules/commander/airspeed_calibration.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d52cafb8f..a2f11b14a 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -168,7 +168,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 10) { + while (calibration_counter < 300) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -182,16 +182,19 @@ int do_airspeed_calibration(int mavlink_fd) float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; - if (fabsf(calibrated_pa) < 9.0f) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", - (double)calibrated_pa); - usleep(3000 * 1000); + if (fabsf(calibrated_pa) < 50.0f) { + if (calibration_counter % 20 == 0) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + (int)calibrated_pa); + } + usleep(100 * 1000); + calibration_counter++; continue; } /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -209,8 +212,8 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)calibrated_pa); break; } -- cgit v1.2.3 From 8590d555b432f206e2b88893ea5198c1398aa99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:44:57 +0200 Subject: Fix calibration counter usage, take every sample --- src/modules/commander/airspeed_calibration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a2f11b14a..10d04aae9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,8 +167,8 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 300) { + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < 1500) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,14 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + calibration_counter++; if (fabsf(calibrated_pa) < 50.0f) { - if (calibration_counter % 20 == 0) { + if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", (int)calibrated_pa); } - usleep(100 * 1000); - calibration_counter++; continue; } -- cgit v1.2.3 From c610050dc305884a704b51c00cfff5f2f9984bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:55:23 +0200 Subject: EKF hotfix: Add missing vector zero calls --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d780..3da92b264 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2280,7 +2280,7 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) } if (!isfinite(val)) { - ekf_debug("constrain: non-finite!"); + //ekf_debug("constrain: non-finite!"); } return ret; @@ -2920,6 +2920,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { -- cgit v1.2.3 From b4b3a2a2c68a523af5141a4452e533befc873384 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:58:57 +0200 Subject: EKF hotfix: Force zero initialization of vectors --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 8 ++++++++ 3 files changed, 11 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 5d768b73d..a835599e7 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 @@ -743,8 +743,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAngRate; + Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3da92b264..a41b58250 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -73,7 +73,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float qUpdated[4]; float quatMag; float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 5648cb05c..0cafdc808 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,6 +19,14 @@ public: float y; float z; + Vector3f() { zero(); } + + Vector3f(float a, float b, float c) : + x(a), + y(b), + z(c) + {} + float length(void) const; void zero(void); }; -- cgit v1.2.3 From 3d505c6f42779eddc983e0f1a25c59d998cdd041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:38:42 +0200 Subject: EKF hotfix: Force all fields to initialized --- .../ekf_att_pos_estimator/estimator_23states.cpp | 153 +++++++++++++++------ 1 file changed, 113 insertions(+), 40 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index a41b58250..7f9486eb5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -5,47 +5,120 @@ #define EKF_COVARIANCE_DIVERGED 1.0e8f -AttPosEKF::AttPosEKF() - - /* NOTE: DO NOT initialize class members here. Use ZeroVariables() - * instead to allow clean in-air re-initialization. - */ +AttPosEKF::AttPosEKF() : + covTimeStepMax(0.0f), + covDelAngMax(0.0f), + rngFinderPitch(0.0f), + a1(0.0f), + a2(0.0f), + a3(0.0f), + yawVarScale(0.0f), + windVelSigma(0.0f), + dAngBiasSigma(0.0f), + dVelBiasSigma(0.0f), + magEarthSigma(0.0f), + magBodySigma(0.0f), + gndHgtSigma(0.0f), + vneSigma(0.0f), + vdSigma(0.0f), + posNeSigma(0.0f), + posDSigma(0.0f), + magMeasurementSigma(0.0f), + airspeedMeasurementSigma(0.0f), + gyroProcessNoise(0.0f), + accelProcessNoise(0.0f), + EAS2TAS(1.0f), + magstate{}, + resetMagState{}, + KH{}, + KHP{}, + P{}, + Kfusion{}, + states{}, + resetStates{}, + storedStates{}, + statetimeStamp{}, + statesAtVelTime{}, + statesAtPosTime{}, + statesAtHgtTime{}, + statesAtMagMeasTime{}, + statesAtVtasMeasTime{}, + statesAtRngTime{}, + statesAtOptFlowTime{}, + correctedDelAng(), + correctedDelVel(), + summedDelAng(), + summedDelVel(), + accNavMag(), + earthRateNED(), + angRate(), + lastGyroOffset(), + delAngTotal(), + Tbn(), + Tnb(), + accel(), + dVelIMU(), + dAngIMU(), + dtIMU(0), + fusionModeGPS(0), + innovVelPos{}, + varInnovVelPos{}, + velNED{}, + accelGPSNED{}, + posNE{}, + hgtMea(0.0f), + baroHgtOffset(0.0f), + rngMea(0.0f), + innovMag{}, + varInnovMag{}, + magData{}, + losData{}, + innovVtas(0.0f), + innovRng(0.0f), + innovOptFlow{}, + varInnovOptFlow{}, + varInnovVtas(0.0f), + varInnovRng(0.0f), + VtasMeas(0.0f), + magDeclination(0.0f), + latRef(0.0f), + lonRef(-M_PI_F), + hgtRef(0.0f), + refSet(false), + magBias(), + covSkipCount(0), + gpsLat(0.0), + gpsLon(-M_PI), + gpsHgt(0.0f), + GPSstatus(0), + baroHgt(0.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + fuseRngData(false), + fuseOptFlowData(false), + + inhibitWindStates(true), + inhibitMagStates(true), + inhibitGndHgtState(true), + + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + useRangeFinder(false), + useOpticalFlow(false), + + ekfDiverged(false), + lastReset(0), + current_ekf_state{}, + last_ekf_error{}, + numericalProtection(true), + storeIndex(0) { - summedDelAng.zero(); - summedDelVel.zero(); - - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - useOpticalFlow = true; - numericalProtection = true; - refSet = false; - storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; - dAngIMU.zero(); - dVelIMU.zero(); - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - accelGPSNED[0] = 0.0f; - accelGPSNED[1] = 0.0f; - accelGPSNED[2] = 0.0f; - delAngTotal.zero(); - ekfDiverged = false; - lastReset = 0; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); -- cgit v1.2.3 From e8855423bea0938c607ed2ab8bde5ebec094b5a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:39:35 +0200 Subject: EKF hotfix: Remove unused variables, use default initializer list for vectors --- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 -- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 +--- 2 files changed, 1 insertion(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index ff311649a..faa6735ca 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -172,8 +172,6 @@ public: unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables - float gpsCourse; - float gpsVelD; double gpsLat; double gpsLon; float gpsHgt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 0cafdc808..6d1f47b68 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,9 +19,7 @@ public: float y; float z; - Vector3f() { zero(); } - - Vector3f(float a, float b, float c) : + Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) : x(a), y(b), z(c) -- cgit v1.2.3 From d4a867071a2183069d8902292357133856cd2ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:44:15 +0200 Subject: airspeed: Better calibration messages --- src/modules/commander/airspeed_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 10d04aae9..469f15632 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -193,7 +193,7 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -206,9 +206,12 @@ int do_airspeed_calibration(int mavlink_fd) } /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", -- cgit v1.2.3 From a1c5f87deaf3e8626f838b191be5b57f33d3dc46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:26:47 +0200 Subject: Fix C++ warnings --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 13 ++++++++++++- src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 7 +++++-- 2 files changed, 17 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 a835599e7..bc40ce3c4 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 @@ -112,6 +112,10 @@ public: */ FixedwingEstimator(); + /* we do not want people ever copying this class */ + FixedwingEstimator(const FixedwingEstimator& that) = delete; + FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + /** * Destructor, also kills the sensors task. */ @@ -362,9 +366,10 @@ FixedwingEstimator::FixedwingEstimator() : _mag_offsets({}), #ifdef SENSOR_COMBINED_SUB - _sensor_combined({}), + _sensor_combined{}, #endif + _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -381,12 +386,18 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _baro_init(false), _gps_initialized(false), + _gps_start_time(0), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), _ekf_logging(true), _debug(0), _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, _ekf(nullptr), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 29a8c8d1e..77cc1eeeb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -45,8 +45,11 @@ void Vector3f::zero(void) z = 0.0f; } -Mat3f::Mat3f() { - identity(); +Mat3f::Mat3f() : + x{1.0f, 0.0f, 0.0f}, + y{0.0f, 1.0f, 0.0f}, + z{0.0f, 0.0f, 1.0f} +{ } void Mat3f::identity() { -- cgit v1.2.3 From 17c5e925fb928d9c0926d8495a2db7a7b464c15f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:08 +0200 Subject: Enable C++ warnings for EKF --- src/modules/ekf_att_pos_estimator/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index dc5220bf0..36d854ddd 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ estimator_23states.cpp \ estimator_utilities.cpp + +EXTRACXXFLAGS = -Weffc++ -- cgit v1.2.3 From 36c20681621c2b99c1486aa0e3700cdbd86bd42d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:42:34 +0200 Subject: Removed spurious submodule entry --- src/modules/ekf_att_pos_estimator/InertialNav | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/modules/ekf_att_pos_estimator/InertialNav (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav deleted file mode 160000 index 8b65d755b..000000000 --- a/src/modules/ekf_att_pos_estimator/InertialNav +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 -- cgit v1.2.3 From 1dffa750d8b91af8f600ba4cf1fbcdcc61edf80f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:44:32 +0200 Subject: added detailed print --- src/modules/commander/airspeed_calibration.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 469f15632..bc12037c3 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,6 +162,8 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 56ad0c708d2695515489802676cc295f040b3606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:53:50 +0200 Subject: Fixed compile error --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index bc12037c3..2e3f89e65 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,7 +162,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 7968c6864e1255b4a65427187119aec2c3fc7ae0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:04:02 +0200 Subject: Force update of offset, do not add offset in final value --- src/modules/commander/airspeed_calibration.cpp | 21 +++++++++++++++------ src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 17 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 2e3f89e65..ea81ac6a6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -140,6 +140,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -182,20 +192,19 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; calibration_counter++; - if (fabsf(calibrated_pa) < 50.0f) { + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ - if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -217,7 +226,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 09ea12c38..372ba9d7d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,8 +668,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; - if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || - hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; -- cgit v1.2.3 From 180b83cc6dc6872c5248993b5409ac835a201114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:41:12 +0200 Subject: Better analog error handling --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index ea81ac6a6..92d50a025 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -94,9 +94,17 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + /* only warn if analog scaling is zero */ + float analog_scaling = 0.0f; + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + if (fabsf(analog_scaling) < 0.1f) { + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); -- cgit v1.2.3 From ec8d395a2d270bd77e873f616a53ee0771c94165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:07:30 +0200 Subject: build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 92d50a025..423d88682 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -97,7 +97,7 @@ int do_airspeed_calibration(int mavlink_fd) /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); -- cgit v1.2.3 From 5f176d14fed9ace9f5265a40b7ace1e6d00a7690 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:33 +0200 Subject: ekf: Only return from start handler with all allocations done --- .../ekf_att_pos_estimator_main.cpp | 25 ++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) (limited to 'src/modules') 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 a835599e7..247814a7e 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 @@ -124,6 +124,13 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + /** * Print the current status. */ @@ -151,7 +158,8 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _estimator_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ #ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ @@ -313,12 +321,13 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator; +FixedwingEstimator *g_estimator = nullptr; } FixedwingEstimator::FixedwingEstimator() : _task_should_exit(false), + _task_running(false), _estimator_task(-1), /* subscriptions */ @@ -772,6 +781,8 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1518,6 +1529,8 @@ FixedwingEstimator::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _estimator_task = -1; @@ -1670,6 +1683,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From 8a0e83c9cd9a54f09c6bc5c2c89917d87a451192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:55 +0200 Subject: fw att ctrl: Only return from start handler with all allocations done --- src/modules/fw_att_control/fw_att_control_main.cpp | 28 ++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) (limited to 'src/modules') 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 3cd4ce928..4cdba735a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 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 @@ -99,13 +98,21 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ @@ -276,6 +283,7 @@ private: * Main sensor collection task. */ void task_main(); + }; namespace att_control @@ -287,12 +295,13 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main() fds[1].fd = _att_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { static int loop_counter = 0; @@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main() warnx("exiting.\n"); _control_task = -1; + _task_running = false; _exit(0); } @@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From efa5d8f57f303a51f618b7c108f516d5e857c3b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:12:17 +0200 Subject: fw pos ctrl: Only return from start handler with all allocations done --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 26 +++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 98ccd09a5..08c996ebc 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 @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 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 @@ -120,10 +119,18 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; @@ -391,13 +398,14 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From 9ce7820e419d2ffa379fb7a3cc168f500623fa3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:32:51 +0200 Subject: Make instructions in commander more obvious for airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 423d88682..598cfe9e2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,15 +180,16 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; + const int maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < 1500) { + while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -204,7 +205,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -212,6 +213,8 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); @@ -246,6 +249,12 @@ int do_airspeed_calibration(int mavlink_fd) } } + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); -- cgit v1.2.3 From e91a4217ad23a0f5190b0e34b7621a5170632a43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:54:12 +0200 Subject: Fix warning --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 598cfe9e2..0e58c68b6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd) usleep(500 * 1000); calibration_counter = 0; - const int maxcount = 3000; + const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { -- cgit v1.2.3 From 1ac2b307e4424d3b6555ab3ca21c43d8de19b81e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 18:23:17 +0200 Subject: Enable stricter compile mode and ensure the most relevant bits are initialized. Needs more work to avoid the remaining warnings --- src/modules/mavlink/mavlink_commands.cpp | 7 +++++-- src/modules/mavlink/mavlink_ftp.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_main.cpp | 10 +++++++++- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_mission.h | 4 ++++ src/modules/mavlink/mavlink_orb_subscription.h | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++------ src/modules/mavlink/mavlink_receiver.h | 4 ++++ src/modules/mavlink/mavlink_stream.h | 4 ++++ src/modules/mavlink/module.mk | 2 ++ 10 files changed, 52 insertions(+), 15 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index fccd4d9a5..b502c3c86 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,9 +40,12 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : + _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd{}, + _channel(channel), + _cmd_time(0) { - _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 675a6870e..6a2c900af 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -50,16 +50,20 @@ MavlinkFTP::getServer() return _server; } -MavlinkFTP::MavlinkFTP() +MavlinkFTP::MavlinkFTP() : + _session_fds{}, + _workBufs{}, + _workFree{}, + _lock{} { // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); - - // initialize session list - for (size_t i=0; i Date: Tue, 15 Jul 2014 23:18:04 +0200 Subject: uORB: Support up to three topics per sensor --- src/modules/uORB/objects_common.cpp | 3 +++ src/modules/uORB/topics/sensor_combined.h | 24 ++++++++++++++++++++++++ 2 files changed, 27 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 679d6ea4f..08c3ce1ac 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -48,14 +48,17 @@ #include ORB_DEFINE(sensor_mag0, struct mag_report); ORB_DEFINE(sensor_mag1, struct mag_report); +ORB_DEFINE(sensor_mag2, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); ORB_DEFINE(sensor_accel1, struct accel_report); +ORB_DEFINE(sensor_accel2, struct accel_report); #include ORB_DEFINE(sensor_gyro0, struct gyro_report); ORB_DEFINE(sensor_gyro1, struct gyro_report); +ORB_DEFINE(sensor_gyro2, struct gyro_report); #include ORB_DEFINE(sensor_baro0, struct baro_report); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index fa3367de9..06dfcdab3 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -95,6 +95,30 @@ struct sensor_combined_s { float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro1_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ + + int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro2_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ -- cgit v1.2.3 From b8b6974e22b7292d5251f1d2c49b2c81e09cc06b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:19:45 +0200 Subject: sdlog2: Add support for up to three IMU sensors --- src/modules/sdlog2/sdlog2.c | 66 ++++++++++++++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 ++++-- 2 files changed, 73 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0d36fa2c6..f534c0f4c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime gyro1_timestamp = 0; + hrt_abstime accelerometer1_timestamp = 0; + hrt_abstime magnetometer1_timestamp = 0; + hrt_abstime gyro2_timestamp = 0; + hrt_abstime accelerometer2_timestamp = 0; + hrt_abstime magnetometer2_timestamp = 0; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; + bool write_IMU1 = false; + bool write_IMU2 = false; bool write_SENS = false; if (buf.sensor.timestamp != gyro_timestamp) { @@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { + accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { + gyro1_timestamp = buf.sensor.gyro1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { + magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; + write_IMU1 = true; + } + + if (write_IMU1) { + log_msg.msg_type = LOG_IMU1_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { + accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { + gyro2_timestamp = buf.sensor.gyro2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { + magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; + write_IMU2 = true; + } + + if (write_IMU2) { + log_msg.msg_type = LOG_IMU2_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + } /* --- ATTITUDE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87f7f718f..b14ef04cc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,8 @@ struct log_ATSP_s { /* --- IMU - IMU SENSORS --- */ #define LOG_IMU_MSG 4 +#define LOG_IMU1_MSG 22 +#define LOG_IMU2_MSG 23 struct log_IMU_s { float acc_x; float acc_y; @@ -276,8 +278,8 @@ struct log_DIST_s { uint8_t flags; }; -// ID 22 available -// ID 23 available +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ + /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -420,7 +422,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), 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"), -- cgit v1.2.3 From b6bac2c88d44fcb7ee0b1fd579f0f0fcc19c2410 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:22:04 +0200 Subject: sensors: Support for up to three sensors of the IMU types --- src/modules/sensors/sensors.cpp | 110 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 107 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 388cd2dcc..4e8a8c01d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -199,9 +199,15 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ - int _gyro_sub; /**< raw gyro data subscription */ - int _accel_sub; /**< raw accel data subscription */ - int _mag_sub; /**< raw mag data subscription */ + int _gyro_sub; /**< raw gyro0 data subscription */ + int _accel_sub; /**< raw accel0 data subscription */ + int _mag_sub; /**< raw mag0 data subscription */ + int _gyro1_sub; /**< raw gyro1 data subscription */ + int _accel1_sub; /**< raw accel1 data subscription */ + int _mag1_sub; /**< raw mag1 data subscription */ + int _gyro2_sub; /**< raw gyro2 data subscription */ + int _accel2_sub; /**< raw accel2 data subscription */ + int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -478,6 +484,12 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _gyro1_sub(-1), + _accel1_sub(-1), + _mag1_sub(-1), + _gyro2_sub(-1), + _accel2_sub(-1), + _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), _vcontrol_mode_sub(-1), @@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; } + + orb_check(_accel1_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer1_m_s2[0] = vect(0); + raw.accelerometer1_m_s2[1] = vect(1); + raw.accelerometer1_m_s2[2] = vect(2); + + raw.accelerometer1_raw[0] = accel_report.x_raw; + raw.accelerometer1_raw[1] = accel_report.y_raw; + raw.accelerometer1_raw[2] = accel_report.z_raw; + + raw.accelerometer1_timestamp = accel_report.timestamp; + } + + orb_check(_accel2_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer2_m_s2[0] = vect(0); + raw.accelerometer2_m_s2[1] = vect(1); + raw.accelerometer2_m_s2[2] = vect(2); + + raw.accelerometer2_raw[0] = accel_report.x_raw; + raw.accelerometer2_raw[1] = accel_report.y_raw; + raw.accelerometer2_raw[2] = accel_report.z_raw; + + raw.accelerometer2_timestamp = accel_report.timestamp; + } } void @@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; } + + orb_check(_gyro1_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro1_rad_s[0] = vect(0); + raw.gyro1_rad_s[1] = vect(1); + raw.gyro1_rad_s[2] = vect(2); + + raw.gyro1_raw[0] = gyro_report.x_raw; + raw.gyro1_raw[1] = gyro_report.y_raw; + raw.gyro1_raw[2] = gyro_report.z_raw; + + raw.gyro1_timestamp = gyro_report.timestamp; + } + + orb_check(_gyro2_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro2_rad_s[0] = vect(0); + raw.gyro2_rad_s[1] = vect(1); + raw.gyro2_rad_s[2] = vect(2); + + raw.gyro2_raw[0] = gyro_report.x_raw; + raw.gyro2_raw[1] = gyro_report.y_raw; + raw.gyro2_raw[2] = gyro_report.z_raw; + + raw.gyro2_timestamp = gyro_report.timestamp; + } } void @@ -1060,6 +1156,8 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + // XXX we need device-id based handling here + if (_mag_is_external) { vect = _external_mag_rotation * vect; @@ -1621,6 +1719,12 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); + _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); -- cgit v1.2.3 From 654aaa0ca852b95e4e2bec5cf9b77ca3242d1d63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:50 +0200 Subject: Mixer: forbid copy constructors due to ptr data members --- src/modules/systemlib/mixer/mixer.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) (limited to 'src/modules') diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 225570fa4..cdf60febc 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -231,6 +231,10 @@ protected: static const char * skipline(const char *buf, unsigned &buflen); private: + + /* do not allow to copy due to prt data members */ + Mixer(const Mixer&); + Mixer& operator=(const Mixer&); }; /** @@ -307,6 +311,10 @@ public: private: Mixer *_first; /**< linked list of mixers */ + + /* do not allow to copy due to pointer data members */ + MixerGroup(const MixerGroup&); + MixerGroup operator=(const MixerGroup&); }; /** @@ -424,6 +432,10 @@ private: mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); + + /* do not allow to copy due to ptr data members */ + SimpleMixer(const SimpleMixer&); + SimpleMixer operator=(const SimpleMixer&); }; /** @@ -522,6 +534,9 @@ private: unsigned _rotor_count; const Rotor *_rotors; + /* do not allow to copy due to ptr data members */ + MultirotorMixer(const MultirotorMixer&); + MultirotorMixer operator=(const MultirotorMixer&); }; #endif -- cgit v1.2.3 From 02f56aae8cf34334580ade6e73bf2d58b12689f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:09 +0200 Subject: Navigator: Enable more strict compile warnings --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 637eaae59..b50198996 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -54,3 +54,5 @@ SRCS = navigator_main.cpp \ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ -- cgit v1.2.3 From 3ca15ab157b395b00ff225e419ae662551bc6b81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:31 +0200 Subject: Controllib block: Make copy constructor private --- src/modules/controllib/block/Block.hpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules') diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 736698e21..9bd80b15b 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -93,6 +93,11 @@ protected: List _subscriptions; List _publications; List _params; + +private: + /* this class has pointer data members and should not be copied (private constructor) */ + Block(const control::Block&); + Block operator=(const control::Block&); }; class __EXPORT SuperBlock : -- cgit v1.2.3 From 63c6e31ba70170029549d60b6ce3b235692ccc80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:50 +0200 Subject: Fix the most obvious compile warnings --- src/modules/navigator/navigator.h | 6 ++++++ src/modules/navigator/navigator_main.cpp | 16 ++++++++-------- src/modules/navigator/navigator_mode.h | 6 ++++++ 3 files changed, 20 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index bf6e2ea0e..8edbb63b3 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -222,5 +222,11 @@ private: * Publish a new position setpoint triplet for position controllers */ void publish_position_setpoint_triplet(); + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + Navigator(const Navigator&); + Navigator operator=(const Navigator&); }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1a5ba4c1a..824ed912d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -106,16 +106,16 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _pos_sp_triplet_pub(-1), - _vstatus({}), - _control_mode({}), - _global_pos({}), - _home_pos({}), - _mission_item({}), - _nav_caps({}), - _pos_sp_triplet({}), + _vstatus{}, + _control_mode{}, + _global_pos{}, + _home_pos{}, + _mission_item{}, + _nav_caps{}, + _pos_sp_triplet{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence({}), + _geofence{}, _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index a7ba79bba..de5545dcb 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -88,6 +88,12 @@ protected: private: bool _first_run; + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + NavigatorMode(const NavigatorMode&); + NavigatorMode operator=(const NavigatorMode&); }; #endif -- cgit v1.2.3 From d39d5cc9da99ec17251428d57d232c30564a663b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 11:26:32 +0200 Subject: SYS_EXT_MAG parameter added for magnetometer selection --- ROMFS/px4fmu_common/init.d/rc.sensors | 27 +++++++++++++++++---------- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 28 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b..f50e9aff7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,12 +6,6 @@ ms5611 start adc start -# Mag might be external -if hmc5883 start -then - echo "[init] Using HMC5883" -fi - if mpu6000 start then echo "[init] Using MPU6000" @@ -22,12 +16,25 @@ then echo "[init] Using L3GD20(H)" fi -if ver hwcmp PX4FMU_V2 +# Use selected (internal/external) magnetometer +if param compare SYS_EXT_MAG 0 then - # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST - if lsm303d start + if hmc5883 -I start + then + echo "[init] Using internal HMC5883" + fi + + if ver hwcmp PX4FMU_V2 + then + if lsm303d start + then + echo "[init] Using internal LSM303D" + fi + fi +else + if hmc5883 -X start then - echo "[init] Using LSM303D" + echo "[init] Using external HMC5883" fi fi diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 702e435ac..8cb923620 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,3 +82,14 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); + +/** +* Set usage of external magnetometer +* +* Set to 1 to use external magnetometer instead of internal one. +* +* @min 0 +* @max 1 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); -- cgit v1.2.3 From 698fcb6b8687b5901145ec90462297c398fbf5c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 14:22:48 +0200 Subject: commander: only warn about lost link if link was working before --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c4d48dea..953feec2a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1441,7 +1441,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); -- cgit v1.2.3 From c6c9c49823a4c19e156f4ce70bde781890ab04f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 14:58:43 +0200 Subject: Implement the external mag param in a fashion that retains backward compatibility --- ROMFS/px4fmu_common/init.d/rc.sensors | 34 ++++++++++++++++++++++------------ src/modules/sensors/sensor_params.c | 13 +++++++++++++ src/modules/systemlib/system_params.c | 11 ----------- 3 files changed, 35 insertions(+), 23 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f50e9aff7..121dc89d3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,33 +8,43 @@ adc start if mpu6000 start then - echo "[init] Using MPU6000" + echo "Internal MPU6000" fi if l3gd20 start then - echo "[init] Using L3GD20(H)" + echo "Internal L3GD20(H)" fi -# Use selected (internal/external) magnetometer -if param compare SYS_EXT_MAG 0 +# MAG selection +if param compare SENS_EXT_MAG 2 then if hmc5883 -I start then - echo "[init] Using internal HMC5883" + echo "Internal HMC5883" fi - - if ver hwcmp PX4FMU_V2 +else + # Use only external as primary + if param compare SENS_EXT_MAG 1 then - if lsm303d start + if hmc5883 -X start + then + echo "External HMC5883" + fi + else + # auto-detect the primary, prefer external + if hmc5883 start then - echo "[init] Using internal LSM303D" + echo "Default HMC5883" fi fi -else - if hmc5883 -X start +fi + +if ver hwcmp PX4FMU_V2 +then + if lsm303d start then - echo "[init] Using external HMC5883" + echo "Internal LSM303D" fi fi diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 38b190761..7ce6ef5ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** +* Set usage of external magnetometer +* +* * Set to 0 (default) to auto-detect (will try to get the external as primary) +* * Set to 1 to force the external magnetometer as primary +* * Set to 2 to force the internal magnetometer as primary +* +* @min 0 +* @max 2 +* @group Sensor Calibration +*/ +PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); + /** * RC Channel 1 Minimum diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 8cb923620..702e435ac 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,14 +82,3 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); - -/** -* Set usage of external magnetometer -* -* Set to 1 to use external magnetometer instead of internal one. -* -* @min 0 -* @max 1 -* @group System -*/ -PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); -- cgit v1.2.3 From d9e3c9423e7d78a71acb31b0a2b3f9cc8b31a55d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 16 Jul 2014 23:41:13 -0400 Subject: Clean up mavlink module Deal with all mavlink module compiler warnings. No critical errors found --- src/modules/mavlink/mavlink_messages.cpp | 144 +++++++++++++++++++++++++++++++ 1 file changed, 144 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7c864f127..6885bebde 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -258,7 +258,16 @@ private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); + protected: + explicit MavlinkStreamHeartbeat() : MavlinkStream(), + status_sub(nullptr), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -322,7 +331,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); + protected: + explicit MavlinkStreamSysStatus() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -384,8 +401,13 @@ private: uint64_t mag_timestamp; uint64_t baro_timestamp; + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + protected: explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_sub(nullptr), sensor_time(0), accel_timestamp(0), gyro_timestamp(0), @@ -469,8 +491,14 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + protected: explicit MavlinkStreamAttitude() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -520,8 +548,13 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + protected: explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -589,12 +622,21 @@ private: MavlinkOrbSubscription *airspeed_sub; uint64_t airspeed_time; + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + protected: explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_sub(nullptr), att_time(0), + pos_sub(nullptr), pos_time(0), + armed_sub(nullptr), armed_time(0), + act_sub(nullptr), act_time(0), + airspeed_sub(nullptr), airspeed_time(0) {} @@ -665,8 +707,13 @@ private: MavlinkOrbSubscription *gps_sub; uint64_t gps_time; + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + protected: explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_sub(nullptr), gps_time(0) {} @@ -726,9 +773,15 @@ private: MavlinkOrbSubscription *home_sub; uint64_t home_time; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + protected: explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_sub(nullptr), pos_time(0), + home_sub(nullptr), home_time(0) {} @@ -789,8 +842,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + protected: explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -845,8 +903,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + protected: explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -899,7 +962,15 @@ public: private: MavlinkOrbSubscription *home_sub; + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + protected: + explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), + home_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); @@ -962,8 +1033,13 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + protected: explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_sub(nullptr), act_time(0) {} @@ -1033,10 +1109,17 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + protected: explicit MavlinkStreamHILControls() : MavlinkStream(), + status_sub(nullptr), status_time(0), + pos_sp_triplet_sub(nullptr), pos_sp_triplet_time(0), + act_sub(nullptr), act_time(0) {} @@ -1159,7 +1242,15 @@ public: private: MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + protected: + explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); @@ -1208,8 +1299,13 @@ private: MavlinkOrbSubscription *pos_sp_sub; uint64_t pos_sp_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + protected: explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_sub(nullptr), pos_sp_time(0) {} @@ -1261,8 +1357,13 @@ private: MavlinkOrbSubscription *att_sp_sub; uint64_t att_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_sub(nullptr), att_sp_time(0) {} @@ -1314,8 +1415,13 @@ private: MavlinkOrbSubscription *att_rates_sp_sub; uint64_t att_rates_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); + MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_sub(nullptr), att_rates_sp_time(0) {} @@ -1367,8 +1473,13 @@ private: MavlinkOrbSubscription *rc_sub; uint64_t rc_time; + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + protected: explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_sub(nullptr), rc_time(0) {} @@ -1456,8 +1567,13 @@ private: MavlinkOrbSubscription *manual_sub; uint64_t manual_time; + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + protected: explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_sub(nullptr), manual_time(0) {} @@ -1510,8 +1626,13 @@ private: MavlinkOrbSubscription *flow_sub; uint64_t flow_time; + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + protected: explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_sub(nullptr), flow_time(0) {} @@ -1563,8 +1684,13 @@ private: MavlinkOrbSubscription *att_ctrl_sub; uint64_t att_ctrl_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + protected: explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_sub(nullptr), att_ctrl_time(0) {} @@ -1626,8 +1752,13 @@ private: MavlinkOrbSubscription *debug_sub; uint64_t debug_time; + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + protected: explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_sub(nullptr), debug_time(0) {} @@ -1678,7 +1809,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + protected: + explicit MavlinkStreamCameraCapture() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -1729,8 +1868,13 @@ private: MavlinkOrbSubscription *range_sub; uint64_t range_time; + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + protected: explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_sub(nullptr), range_time(0) {} -- cgit v1.2.3 From 878fd1d7134fb53c358c0289ff25c9a6633c73ae Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 17 Jul 2014 09:50:11 +0200 Subject: navigator: members initialization fixed --- src/modules/navigator/navigator_main.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 824ed912d..93cbfd7b1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -105,6 +105,7 @@ Navigator::Navigator() : _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), + _param_update_sub(-1), _pos_sp_triplet_pub(-1), _vstatus{}, _control_mode{}, @@ -124,6 +125,8 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), + _can_loiter_at_sp(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { -- cgit v1.2.3 From facffe2b9eeee24f712119be91ff8aea99cfde0f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 17:38:04 +0200 Subject: mtecs: add altitude prefiltering --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 10 ++++++++-- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 +++++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 20 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 2e37d166e..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,6 +59,7 @@ mTecs::mTecs() : _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), _flightPathAngleLowpass(this, "FPA_LP"), + _altitudeLowpass(this, "ALT_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* time measurement */ updateTimeMeasurement(); + /* Filter altitude */ + float altitudeFiltered = _altitudeLowpass.update(altitude); + + /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } /* Write part of the status message */ _status.altitudeSp = altitudeSp; _status.altitude = altitude; + _status.altitudeFiltered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index efa89a5d3..ae6867d38 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -115,6 +115,7 @@ protected: /* Other calculation Blocks */ control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 5b9238780..4ca31fe20 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); +/** + * Lowpass (cutoff freq.) for altitude + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); + /** * Lowpass (cutoff freq.) for the flight path angle * diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4c..e0a3e7603 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1595,6 +1595,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b14ef04cc..853a3811f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -334,6 +334,7 @@ struct log_GS1B_s { struct log_TECS_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; @@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = { 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"), - LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index c4d0c1874..33055018c 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -66,6 +66,7 @@ struct tecs_status_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; -- cgit v1.2.3 From c01567c047f295e3ffea7d01a7e239ac4f79a498 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:02:17 +0200 Subject: Check if waypoint altitude is relative. Fixes #1197 --- src/modules/navigator/mission_feasibility_checker.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c801..d45488d5a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return true; } } -- cgit v1.2.3 From 101e7b1383b83c9b06ef147dbef1ff375c11b84f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:06:42 +0200 Subject: navigator: Feedback strings / text and logic was not consistent in previous state, fixed. --- src/modules/navigator/mission_feasibility_checker.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d45488d5a..606521f20 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -136,14 +136,14 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } /* calculate the global waypoint altitude */ - float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } } -- cgit v1.2.3 From 9559668f0fb7477e2f777beff899589a387d26c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:25:12 +0200 Subject: uORB: Add navigation state --- src/modules/uORB/topics/position_setpoint_triplet.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 673c0e491..4a1932180 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -95,6 +95,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + unsigned nav_state; /**< report the navigation state */ }; /** -- cgit v1.2.3 From b8da27561613a0890fd21e43064b457a58daaab3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:25:33 +0200 Subject: Log nav mode --- src/modules/sdlog2/sdlog2.c | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4c..e078b47d6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); + + if (buf.triplet.current.valid) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } } /* --- VICON POSITION --- */ -- cgit v1.2.3 From ba7df4c170e069812387e08445cfa0e7789bf34d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:26:04 +0200 Subject: Enable nav mode logging in navigator --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 93cbfd7b1..331a9a728 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -458,7 +458,7 @@ void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - /* TODO: set nav_state */ + _pos_sp_triplet.nav_state = _vstatus.nav_state; /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { -- cgit v1.2.3 From dc612d75c788bc8b2a90c200a13a5a5a0e3a68d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 18 Jul 2014 00:15:16 +0200 Subject: BlockDerivative: initialize in first run --- src/modules/controllib/blocks.cpp | 7 ++++++- src/modules/controllib/blocks.hpp | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index c6c374300..04cb023a8 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,7 +293,12 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = _lowPass.update((input - getU()) / getDt()); + float output = 0.0f; + if (_initialized) { + output = _lowPass.update((input - getU()) / getDt()); + } else { + _initialized = true; + } setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 66e929038..b0545b60a 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -238,6 +238,7 @@ public: BlockDerivative(SuperBlock *parent, const char *name) : SuperBlock(parent, name), _u(0), + _initialized(false), _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; @@ -249,6 +250,7 @@ public: protected: // attributes float _u; /**< previous input */ + bool _initialized; BlockLowPass _lowPass; /**< low pass filter */ }; -- cgit v1.2.3 From 6c50e510a5fcadab19a6d5ff709c0da473e4ace0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:28:49 +0200 Subject: Derivative fix: Comments and code style --- src/modules/controllib/blocks.cpp | 8 +++++++- src/modules/controllib/blocks.hpp | 15 +++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 04cb023a8..0175acda9 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,10 +293,16 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = 0.0f; + float output; if (_initialized) { output = _lowPass.update((input - getU()) / getDt()); } else { + // if this is the first call to update + // we have no valid derivative + // and so we use the assumption the + // input value is not changing much, + // which is the best we can do here. + output = 0.0f; _initialized = true; } setU(input); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index b0545b60a..37d7832b3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -242,6 +242,21 @@ public: _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ float update(float input); // accessors void setU(float u) {_u = u;} -- cgit v1.2.3 From ce78b399691d43bd150c0a5928f08c98076a3892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:49:29 +0200 Subject: Remove all unused TECS parameters --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 54 ------- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 169 --------------------- 2 files changed, 223 deletions(-) (limited to 'src/modules') 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 6c251c237..1fcab2069 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 @@ -199,19 +199,6 @@ private: float l1_period; float l1_damping; - float time_const; - float min_sink_rate; - float max_sink_rate; - float max_climb_rate; - float throttle_damp; - float integrator_gain; - float vertical_accel_limit; - float height_comp_filter_omega; - float speed_comp_filter_omega; - float roll_throttle_compensation; - float speed_weight; - float pitch_damping; - float airspeed_min; float airspeed_trim; float airspeed_max; @@ -242,19 +229,6 @@ private: param_t l1_period; param_t l1_damping; - param_t time_const; - param_t min_sink_rate; - param_t max_sink_rate; - param_t max_climb_rate; - param_t throttle_damp; - param_t integrator_gain; - param_t vertical_accel_limit; - param_t height_comp_filter_omega; - param_t speed_comp_filter_omega; - param_t roll_throttle_compensation; - param_t speed_weight; - param_t pitch_damping; - param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -470,21 +444,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); - _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); - _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); - _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); - _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); - _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); - _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); - _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); - _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); - _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); - _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); - _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); - _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); - _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); - /* fetch initial parameter values */ parameters_update(); } @@ -535,19 +494,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); - param_get(_parameter_handles.time_const, &(_parameters.time_const)); - param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); - param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); - param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); - param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); - param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); - param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); - param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); - param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); - param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); - param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); - param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); - param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); 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 52128e1b7..35b1f2ca9 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 @@ -154,175 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); -/** - * Maximum climb rate - * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or - * FW_THR_MAX reduced. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - -/** - * Minimum descent rate - * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used - * to measure FW_T_CLMB_MAX. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); - -/** - * Maximum descent rate - * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding - * the aircraft. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/** - * TECS time constant - * - * This is the time constant of the TECS control algorithm (in seconds). - * Smaller values make it faster to respond, larger values make it slower - * to respond. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/** - * Throttle damping factor - * - * This is the damping gain for the throttle demand loop. - * Increase to add damping to correct for oscillations in speed and height. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - -/** - * Integrator gain - * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and - * increases overshoot. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - -/** - * Maximum vertical acceleration - * - * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover - * from under-speed conditions. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/** - * Complementary filter "omega" parameter for height - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights - * the solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - -/** - * Complementary filter "omega" parameter for speed - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an - * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the - * solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - -/** - * Roll -> Throttle feedforward - * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas - * inefficient low aspect-ratio models (eg delta wings) can use a higher value. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - -/** - * Speed <--> Altitude priority - * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will - * adjust its pitch angle to maintain airspeed, ignoring changes in height). - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - -/** - * Pitch damping factor - * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned - * properly. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -/** - * Height rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); - /** * Speed rate P factor * -- cgit v1.2.3 From 4a8e4d37cd926bed7b85ba92a5214f604a4ea192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:55:02 +0200 Subject: Remove one left-over TECS param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ------- 1 file changed, 7 deletions(-) (limited to 'src/modules') 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 35b1f2ca9..27039ff51 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 @@ -154,13 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); -/** - * Speed rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); - /** * Landing slope angle * -- cgit v1.2.3 From 90a5ae1afd25e5e31d269f0d0f5e5052f068d0b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 08:04:13 +0200 Subject: Remove MT_ENABLED param and handles --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 -- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 11 ----------- 3 files changed, 14 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..8f0e57e20 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -49,7 +49,6 @@ namespace fwPosctrl { mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ - _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ _status(&getPublications(), ORB_ID(tecs_status)), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ae6867d38..99830fba6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -90,14 +90,12 @@ public: void resetDerivatives(float airspeed); /* Accessors */ - bool getEnabled() { return _mTecsEnabled.get() > 0; } float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ - control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe20..6e9e1d88e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -46,17 +46,6 @@ * Controller parameters, accessible via MAVLink */ -/** - * mTECS enabled - * - * Set to 1 to enable mTECS - * - * @min 0 - * @max 1 - * @group mTECS - */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); - /** * Total Energy Rate Control Feedforward * Maps the total energy rate setpoint to the throttle setpoint -- cgit v1.2.3 From 81f60329e47f8b31d9261c0ef46c09780f9d8194 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 08:07:35 +0200 Subject: Remove last reference to mtecs enabled param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules') 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 1fcab2069..5894aeb0b 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 @@ -756,10 +756,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - } + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } _was_pos_control_mode = true; -- cgit v1.2.3 From 9a53fd96482bd31da98af97de5cde88127d6c7f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 11:18:32 +0200 Subject: Add force failsafe flag --- src/modules/px4iofirmware/mixer.cpp | 9 ++++++++- src/modules/px4iofirmware/protocol.h | 5 +++++ 2 files changed, 13 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2f721bf1e..606c639f9 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -111,7 +111,7 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } - /* default to failsafe mixing */ + /* default to failsafe mixing - it will be forced below if flag is set */ source = MIX_FAILSAFE; /* @@ -154,6 +154,13 @@ mixer_tick(void) } } + /* + * Check if we should force failsafe - and do it if we have to + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { + source = MIX_FAILSAFE; + } + /* * Set failsafe status flag depending on mixing source */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d5a6b1ec4..050783687 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -179,6 +179,7 @@ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -253,6 +254,10 @@ enum { /* DSM bind states */ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + /* Debug and test page - not used in normal operation */ #define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ -- cgit v1.2.3 From a2da34be16531404b4c4507ab71b0518a51d60c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 20:53:01 +0200 Subject: Fixes #1207 --- src/modules/mavlink/mavlink_main.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e235ff0f9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -525,22 +525,39 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; -- cgit v1.2.3 From a4e1a665833f566e2faaa80d221971c679b57d17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 10:52:50 +0200 Subject: Revert "Remove last reference to mtecs enabled param" This reverts commit 81f60329e47f8b31d9261c0ef46c09780f9d8194. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules') 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 5894aeb0b..1fcab2069 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 @@ -756,8 +756,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } } _was_pos_control_mode = true; -- cgit v1.2.3 From 57794925fe0fcaf5736163f8ad43a4551a1a9b71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 10:53:04 +0200 Subject: Revert "Remove MT_ENABLED param and handles" This reverts commit 90a5ae1afd25e5e31d269f0d0f5e5052f068d0b1. --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 ++ src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 11 +++++++++++ 3 files changed, 14 insertions(+) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 8f0e57e20..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -49,6 +49,7 @@ namespace fwPosctrl { mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ + _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ _status(&getPublications(), ORB_ID(tecs_status)), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 99830fba6..ae6867d38 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -90,12 +90,14 @@ public: void resetDerivatives(float airspeed); /* Accessors */ + bool getEnabled() { return _mTecsEnabled.get() > 0; } float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 6e9e1d88e..4ca31fe20 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -46,6 +46,17 @@ * Controller parameters, accessible via MAVLink */ +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 1); + /** * Total Energy Rate Control Feedforward * Maps the total energy rate setpoint to the throttle setpoint -- cgit v1.2.3 From 322089a390f416a9936fb4ad1ad0ed4359aeaa57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 11:29:49 +0200 Subject: Fix unused params --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 --------- 1 file changed, 9 deletions(-) (limited to 'src/modules') 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 1fcab2069..0b111f7bd 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 @@ -212,9 +212,6 @@ private: float throttle_land_max; - float heightrate_p; - float speedrate_p; - float land_slope_angle; float land_H1_virt; float land_flare_alt_relative; @@ -242,9 +239,6 @@ private: param_t throttle_land_max; - param_t heightrate_p; - param_t speedrate_p; - param_t land_slope_angle; param_t land_H1_virt; param_t land_flare_alt_relative; @@ -494,9 +488,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); - param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); - param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); - param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); -- cgit v1.2.3 From 689536893caea0cde8cafb3a51cd7e471338cfb0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 12:46:52 +0200 Subject: px4io driver: force failsafe depending on actuator armed --- src/drivers/px4io/px4io.cpp | 20 +++++++++++++------- src/modules/uORB/topics/actuator_armed.h | 3 ++- 2 files changed, 15 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5ff6e55e..5ada635f3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } + if (armed.force_failsafe) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2435,7 +2441,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { - /* check IO firmware CRC against passed value */ + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); if (ret != OK) @@ -2695,7 +2701,7 @@ checkcrc(int argc, char *argv[]) int fd = open(argv[1], O_RDONLY); if (fd == -1) { printf("open of %s failed - %d\n", argv[1], errno); - exit(1); + exit(1); } const uint32_t app_size_max = 0xf000; uint32_t fw_crc = 0; @@ -2710,7 +2716,7 @@ checkcrc(int argc, char *argv[]) close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); + fw_crc = crc32part(&b, 1, fw_crc); nbytes++; } @@ -2723,7 +2729,7 @@ checkcrc(int argc, char *argv[]) if (ret != OK) { printf("check CRC failed - %d\n", ret); - exit(1); + exit(1); } printf("CRCs match\n"); exit(0); @@ -2753,12 +2759,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2960,7 +2966,7 @@ lockdown(int argc, char *argv[]) (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); warnx("ACTUATORS ARE NOW SAFE IN HIL."); } - + } else { errx(1, "driver not loaded, exiting"); } diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index a98d3fc3a..0f6c9aca1 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -56,6 +56,7 @@ struct actuator_armed_s { 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) */ + bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ }; /** @@ -65,4 +66,4 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); -#endif \ No newline at end of file +#endif -- cgit v1.2.3 From 3a0fc36c67ef89faa0b20e847eb16de6362ef3e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:12:33 +0200 Subject: Consider the throttle load for battery voltage calculation --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/commander/commander_helper.cpp | 10 +++++++--- src/modules/commander/commander_helper.h | 3 ++- src/modules/commander/commander_params.c | 4 ++-- 4 files changed, 21 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a..7332c6eaa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -918,6 +918,11 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + /* Subscribe to actuator controls (outputs) */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1199,13 +1204,17 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); + + /* get throttle (if armed), as we only care about energy negative throttle also counts */ + float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d5fe122cb..a4aafa1f6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) } } -float battery_remaining_estimate_voltage(float voltage, float discharged) +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; static param_t bat_capacity_h; + static param_t bat_v_load_drop_h; static float bat_v_empty = 3.2f; static float bat_v_full = 4.0f; + static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; @@ -300,20 +302,22 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) bat_v_full_h = param_find("BAT_V_FULL"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); initialized = true; } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); param_get(bat_v_full_h, &bat_v_full); + param_get(bat_v_load_drop_h, &bat_v_load_drop); param_get(bat_n_cells_h, &bat_n_cells); param_get(bat_capacity_h, &bat_capacity); } counter++; - /* remaining charge estimate based on voltage */ - float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ + float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index a49c9e263..4a77fe487 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * * @param voltage the current battery voltage * @param discharged the discharged capacity + * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy. * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage, float discharged); +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5c..448bc53cd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); /** * Full cell voltage. @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); /** * Number of cells. -- cgit v1.2.3 From 3db78a4ab31b79f092940f4186549132a40ed855 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:34:19 +0200 Subject: commander: Warn at 18 and 9% battery remaining instead of 25 and 10% to not trigger the warning too early (at 50%) as it was before --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7332c6eaa..cb447f8ce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1276,13 +1276,13 @@ int commander_thread_main(int argc, char *argv[]) } /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); -- cgit v1.2.3 From 5fc3bc787a495d15099e177b99c2c4d6a1a4f56c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c5..adca0e88f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; -- cgit v1.2.3 From 0b743a9f5c29cee3b3d7c6d602091453e1091973 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 14:39:41 +0200 Subject: parse flighttermination command --- src/modules/commander/commander.cpp | 21 ++++++++------------- src/modules/uORB/topics/vehicle_command.h | 1 + 2 files changed, 9 insertions(+), 13 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a..04450a44f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; -#if 0 /* Flight termination */ - case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - - //XXX: to enable the parachute, a param needs to be set - //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { - transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - + case VEHICLE_CMD_DO_FLIGHTTERMINATION: { + if (cmd->param1 > 0.5f) { + //XXX update state machine? + armed_local->force_failsafe = true; + warnx("forcing failsafe"); } else { - /* reject parachute depoyment not armed */ - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + armed_local->force_failsafe = false; + warnx("disabling failsafe"); } - + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; -#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b13..7db33d98b 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -76,6 +76,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ -- cgit v1.2.3 From 730a520362caf9c9d3e506a31441d9921e008144 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e25851c7d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f..d51120462 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34d..528c8b72a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; -- cgit v1.2.3 From 87cc7a81ff8b3bdc9cc5a5b4fca91f8d08988774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:08:56 +0200 Subject: Support force fail in valid filter --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b37259997..fcd53f1f1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN) + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) -- cgit v1.2.3 From ed31d6a59a21753e326ba03933ce0eb962b8412a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++-------- src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 ++- 4 files changed, 32 insertions(+), 24 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d..0e8f17c77 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { + printf("\tno telem status.\n"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a..454d730d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + hrt_abstime tnow = hrt_absolute_time(); + + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; /* if no radio status messages arrive, lets at least publish that heartbeats were received */ if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; if (_telemetry_status_pub < 0) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3..f4d0c52d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,7 +148,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; int _control_mode_sub; int _hil_frames; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a5..1297c1a9d 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ -- cgit v1.2.3 From 5ace06f3b820d43bc313347f3d23b121a36fbef1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77..7bd2b9754 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { -- cgit v1.2.3 From 6e3ebd3a36ce8bf3a9132b76986074ad8d9fcf35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 170 +++++++++++++++++++---------------- 1 file changed, 94 insertions(+), 76 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754..cb8c298c0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,7 +81,7 @@ #include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - default: + + default: return; } @@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { /* 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]) - { + 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); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ Mavlink::Mavlink() : _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), - _logbuffer{}, - _total_counter(0), - _receive_thread{}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(10000), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -867,8 +870,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_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)) { + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); @@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_set.target_system == mavlink_system.sysid + && ((mavlink_param_set.target_component == mavlink_system.compid) + || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_request_read.target_system == mavlink_system.sysid + && ((mavlink_param_request_read.target_component == mavlink_system.compid) + || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ @@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string) /* Map severity */ switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; } mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); @@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1475,7 +1487,8 @@ 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, (double)_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); @@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[]) bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { printf("\tno telem status.\n"); } -- cgit v1.2.3 From 52a9513e64dacf1004fd0e11df3daf5eb607b0f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:22:02 +0200 Subject: Update rates --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c0..2e3a9ca33 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1454,7 +1454,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; -- cgit v1.2.3 From 41003a243793e465a71e9b142e7c9aac132d3923 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2e3a9ca33..ea60bb226 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ instance->count_txerr(); + instance->count_txerrbytes(desired); return; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ret != desired) { instance->count_txerr(); + instance->count_txerrbytes(desired); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; + instance->count_txbytes(desired); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } + perf_end(_loop_perf); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462..70d13acb0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ public: */ void count_txerr(); + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** * Get the receive status of this MAVLink link */ @@ -293,6 +308,14 @@ private: bool _flow_control_enabled; + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d8..54c412ce7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } -- cgit v1.2.3 From 5d2489880c2a42bc67554efa7715ba3a761c5d17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 21:14:14 +0200 Subject: Fix low voltage warning threshold to 9%, not 90% --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb447f8ce..562e9a1b6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1282,7 +1282,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 448bc53cd..a09e2d6bf 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); /** * Full cell voltage. -- cgit v1.2.3 From e7d8d9c91fe72169d292a5e0ac44133a218f27a0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 23:19:49 +0200 Subject: position_estimator_inav: parameters descriptions added --- .../position_estimator_inav_params.c | 174 +++++++++++++++++++++ 1 file changed, 174 insertions(+) (limited to 'src/modules') 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 0581f8236..98b31d17b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,22 +40,196 @@ #include "position_estimator_inav_params.h" +/** + * Z axis weight for barometer + * + * Weight (cutoff frequency) for barometer altitude measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); + +/** + * Z axis weight for GPS + * + * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); + +/** + * Z axis weight for sonar + * + * Weight (cutoff frequency) for sonar measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); + +/** + * XY axis weight for GPS position + * + * Weight (cutoff frequency) for GPS position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); + +/** + * XY axis weight for GPS velocity + * + * Weight (cutoff frequency) for GPS velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); + +/** + * XY axis weight for optical flow + * + * Weight (cutoff frequency) for optical flow (velocity) measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); + +/** + * XY axis weight for resetting velocity + * + * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); + +/** + * XY axis weight factor for GPS when optical flow available + * + * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); + +/** + * Accelerometer bias estimation weight + * + * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + * + * @min 0.0 + * @max 0.1 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); + +/** + * Optical flow scale factor + * + * Factor to convert raw optical flow (in pixels) to radians [rad/px]. + * + * @min 0.0 + * @max 1.0 + * @unit rad/px + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); + +/** + * Minimal acceptable optical flow quality + * + * 0 - lowest quality, 1 - best quality. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); + +/** + * Weight for sonar filter + * + * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); + +/** + * Sonar maximal error for new surface + * + * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + * + * @min 0.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); + +/** + * Land detector time + * + * Vehicle assumed landed if no altitude changes happened during this time on low throttle. + * + * @min 0.0 + * @max 10.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); + +/** + * Land detector altitude dispersion threshold + * + * Dispersion threshold for triggering land detector. + * + * @min 0.0 + * @max 10.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); + +/** + * Land detector throttle threshold + * + * Value should be lower than minimal hovering thrust. Half of it is good choice. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); + +/** + * GPS delay + * + * GPS delay compensation + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) -- cgit v1.2.3 From cd8a0cd21771fb3a6fb8cddfc3da322eb04f10db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 23:39:02 +0200 Subject: mavlink: Only send event-based messages if there is space in the buffer --- src/modules/mavlink/mavlink_main.cpp | 60 +++++++++++++++++++++++++++++++++--- src/modules/mavlink/mavlink_main.h | 17 +++++++++- src/modules/mavlink/mavlink_stream.h | 13 ++++++++ 3 files changed, 84 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ea60bb226..e6b558ca6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -246,7 +246,8 @@ Mavlink::Mavlink() : _ftp_on(false), _uart_fd(-1), _baudrate(57600), - _datarate(10000), + _datarate(1000), + _datarate_events(500), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -352,6 +353,18 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } +unsigned +Mavlink::get_free_tx_buf() +{ + unsigned buf_free; + + if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { + return buf_free; + } else { + return 0; + } +} + void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -1083,6 +1096,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return ERROR; } +void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { @@ -1464,7 +1502,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; @@ -1519,10 +1557,22 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - _mission_manager->eventloop(); - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { + unsigned buf_free = get_free_tx_buf(); + + /* only send messages if they fit the buffer */ + if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + mavlink_pm_queued_send(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + _mission_manager->eventloop(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && + (!mavlink_logbuffer_is_empty(&_logbuffer))) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 70d13acb0..e9d6b9c86 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -247,6 +247,13 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + /** * Get the receive status of this MAVLink link */ @@ -292,7 +299,8 @@ private: bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages /** * If the queue index is not at 0, the queue sending @@ -385,6 +393,13 @@ private: int configure_stream(const char *stream_name, const float rate); + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + int message_buffer_init(int size); void message_buffer_destroy(); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 20e1c7c44..f539d225b 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -56,7 +56,20 @@ public: MavlinkStream(); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } void set_channel(mavlink_channel_t channel); /** -- cgit v1.2.3 From 1f3625371d787cdc452b45ad9d9a01423ae51f96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 10:18:00 +0200 Subject: Obey radio status in opportunistic transmissions as well. Last missing bit are adaptive rates --- src/modules/mavlink/mavlink_main.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6b558ca6..e7e96dc3a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -359,7 +359,34 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - return buf_free; + if (_rstatus.timestamp > 0 && + (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { + + unsigned low_buf; + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + low_buf = 50; + break; + default: + low_buf = 50; + break; + } + + if (_rstatus.txbuf < low_buf) { + /* + * If the TX buf measure is initialized and up to date and low + * return 0 to slow event based transmission + */ + return 0; + } else { + /* there is no SW flow control option, just use what our buffer tells us */ + return buf_free; + } + + } else { + return buf_free; + } } else { return 0; } -- cgit v1.2.3 From 5fb2a92e7704c2e298128addf890722b6b03289a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:05 +0200 Subject: commander: Do not confuse developers with wrong comments, do not confuse users with not at all helpful "general error" messages. --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44f..48cbc254f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,12 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "arming state transition denied"); + /* + * the arming transition can be denied to a number of reasons: + * - pre-flight check failed (sensors not ok or not calibrated) + * - safety not disabled + * - system not in manual mode + */ tune_negative(true); } -- cgit v1.2.3 From b3a80025b3bf514e987d19b7292ba0f9d16d7d1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:24 +0200 Subject: Do not confuse operators / users with technical error messages --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 372ba9d7d..4ca8471fc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,11 +216,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "INVAL: %s - %s"; - - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + /* only print to console here as this is too technical to be useful during operation */ + warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } return ret; -- cgit v1.2.3 From f3b8890601e40e5131c55e6f96ad1452a53410eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:31:23 +0200 Subject: commander: Explain sensor arming fail case to users --- src/modules/commander/state_machine_helper.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4ca8471fc..16ed8dd52 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -200,6 +200,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); valid_transition = false; } -- cgit v1.2.3 From a5f538dc5c9570b59c7135570a662651cb857698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:06:31 +0200 Subject: Commander: Print technical feedback as last resort if no feedback was provided before --- src/modules/commander/state_machine_helper.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 16ed8dd52..7b26e3e8c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { @@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if pre-arm check fails if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); - + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; valid_transition = false; } @@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (!status->condition_power_input_valid) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; valid_transition = false; } @@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current (status->avionics_power_rail_voltage < 4.9f))) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + feedback_provided = true; valid_transition = false; } } @@ -201,6 +205,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; valid_transition = false; } @@ -217,8 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - /* only print to console here as this is too technical to be useful during operation */ - warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } } return ret; @@ -646,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); - mavlink_log_critical(mavlink_fd, "hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 3935540c7df0f67946b637af6dc2d3145453c326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:23:43 +0200 Subject: Set full voltage correctly to ensure percentage range fits. Force all users to new value by param renaming. Since this will tend to show batteries as more drained than before, this is a change in a safe direction and will not trigger unnoticed discharges. --- src/modules/commander/commander_helper.cpp | 6 +++--- src/modules/commander/commander_params.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a4aafa1f6..239b77df0 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -289,8 +289,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_n_cells_h; static param_t bat_capacity_h; static param_t bat_v_load_drop_h; - static float bat_v_empty = 3.2f; - static float bat_v_full = 4.0f; + static float bat_v_empty = 3.4f; + static float bat_v_full = 4.2f; static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; @@ -299,7 +299,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float if (!initialized) { bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_FULL"); + bat_v_full_h = param_find("BAT_V_CHARGED"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a09e2d6bf..dea12db40 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); +PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); /** * Number of cells. -- cgit v1.2.3 From 264fe884a2825ae430a19ba61e8c89d6a214f5cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:28:44 +0200 Subject: Fixed load voltage calculation --- src/modules/commander/commander_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 239b77df0..d87af2d7d 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -317,7 +317,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); + float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ -- cgit v1.2.3 From c95de36d3a01bda1d3664fbc46df3a5fc35fe4da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 19:42:31 +0200 Subject: commander: Add missing parameter definition --- src/modules/commander/commander_helper.cpp | 2 +- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d87af2d7d..2022e99fb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -291,7 +291,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_v_load_drop_h; static float bat_v_empty = 3.4f; static float bat_v_full = 4.2f; - static float bat_v_load_drop = 0.1f; + static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index dea12db40..0e4973b5f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -67,6 +67,16 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); +/** + * Voltage drop per cell on 100% load + * + * This implicitely defines the internal resistance + * to maximum current ratio and assumes linearity. + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); + /** * Number of cells. * -- cgit v1.2.3 From ee7cd04e4c480a1931223889081074c469be0dbb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Jul 2014 20:34:32 +0200 Subject: mavlink: use send_message() method of Mavlink class instead of using helpers --- src/modules/mavlink/mavlink_main.cpp | 237 +++++++++++++------------------ src/modules/mavlink/mavlink_main.h | 6 + src/modules/mavlink/mavlink_messages.cpp | 32 ++--- src/modules/mavlink/mavlink_stream.cpp | 13 +- src/modules/mavlink/mavlink_stream.h | 8 +- 5 files changed, 123 insertions(+), 173 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e7e96dc3a..e3c452fb2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,11 +94,15 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP 256 + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; + /** * mavlink app start / stop handling function * @@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; -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 - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - - /* 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)) { - - if (buf_free < desired) { - /* we don't want to send anything just in half, so return */ - instance->count_txerr(); - instance->count_txerrbytes(desired); - return; - } - } - - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - instance->count_txerr(); - instance->count_txerrbytes(desired); - - } else { - last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; - instance->count_txbytes(desired); - } - } -} - static void usage(void); Mavlink::Mavlink() : @@ -253,6 +150,8 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -359,30 +258,10 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.timestamp > 0 && - (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { - - unsigned low_buf; - - switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - low_buf = 50; - break; - default: - low_buf = 50; - break; - } + if (_rstatus.telem_time > 0 && + (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - if (_rstatus.txbuf < low_buf) { - /* - * If the TX buf measure is initialized and up to date and low - * return 0 to slow event based transmission - */ - return 0; - } else { - /* there is no SW flow control option, just use what our buffer tells us */ - return buf_free; - } + return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); } else { return buf_free; @@ -808,13 +687,93 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } +bool +Mavlink::check_can_send(const int prio, const unsigned packet_len) +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (get_flow_control_enabled() + && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) { + + /* 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_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + 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 (should_transmit()) { + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + + if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { + warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); + /* we don't want to send anything just in half, so return */ + count_txerr(); + count_txerrbytes(packet_len); + return true; + } + } + return false; +} + void -Mavlink::send_message(const mavlink_message_t *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (!check_can_send(prio, packet_len)) { + return; + } + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } } void @@ -1585,21 +1544,17 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { - unsigned buf_free = get_free_tx_buf(); - /* only send messages if they fit the buffer */ - if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { mavlink_pm_queued_send(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { _mission_manager->eventloop(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && - (!mavlink_logbuffer_is_empty(&_logbuffer))) { + if (!mavlink_logbuffer_is_empty(&_logbuffer) && + check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e9d6b9c86..1c62b03d2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -160,6 +160,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + bool check_can_send(const int prio, const unsigned packet_len); + + void send_message(const uint8_t msgid, const void *msg, const int prio); + void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -315,6 +319,8 @@ private: float _subscribe_to_stream_rate; bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; unsigned _bytes_tx; unsigned _bytes_txerr; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..fe33985d2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -249,9 +249,9 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); } private: @@ -263,15 +263,15 @@ private: MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - explicit MavlinkStreamHeartbeat() : MavlinkStream(), + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr), pos_sp_triplet_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); } void send(const hrt_abstime t) @@ -290,17 +290,15 @@ protected: memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + mavlink_heartbeat_t msg; + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = mavlink_system.type; + msg.autopilot = mavlink_system.type; + msg.mavlink_version = 3; + + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); } }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206db..79dc23cc6 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval) _interval = interval; } -/** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - /** * Update subscriptions and send message if necessary */ diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index f539d225b..7d3afbbb5 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -54,7 +54,7 @@ class MavlinkStream public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); /** @@ -76,14 +76,14 @@ public: * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; + virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; -- cgit v1.2.3 From 25d1cc3995f3ecafe2408582296d6dedfe49ce53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 07:35:23 +0200 Subject: Final value for battery load param default --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0e4973b5f..dba68700b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); /** * Number of cells. -- cgit v1.2.3 From 956c084f31af9ec2aa80d61eee17e7506b4c7172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:31:35 +0200 Subject: Fit IO voltage estimation using a new dataset, cross-validated with a second unit. Pending testing --- src/modules/px4iofirmware/registers.c | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fcd53f1f1..7fbb0ecf1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -739,30 +739,17 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { /* * Coefficients here derived by measurement of the 5-16V - * range on one unit: + * range on one unit, validated on sample points of another unit * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * Data in Tools/tests-host/data folder. * - * slope = 0.0046067 - * intercept = 0.3863 + * slope = 0.004585267878277 (int: 4585) + * intercept = 0.016646394188076 (int: 16646 * - * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned mV = (16646 + (counts * 4585)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; -- cgit v1.2.3 From 0fc73a1484ac9e9cbc2f52ec7e81b28a109777eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:46:38 +0200 Subject: Fix comment, missing brace in comment --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7fbb0ecf1..59b3043aa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -744,7 +744,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Data in Tools/tests-host/data folder. * * slope = 0.004585267878277 (int: 4585) - * intercept = 0.016646394188076 (int: 16646 + * intercept = 0.016646394188076 (int: 16646) * */ unsigned counts = adc_measure(ADC_VBATT); -- cgit v1.2.3 From 178b0f7399ab881e44f2d2ecff809aea53a4397d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 19:25:23 +0200 Subject: Cross-check with nominal values from divider --- src/modules/px4iofirmware/registers.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 59b3043aa..8c15c66c1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -743,13 +743,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * * Data in Tools/tests-host/data folder. * - * slope = 0.004585267878277 (int: 4585) + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) * */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (16646 + (counts * 4585)) / 1000; + unsigned mV = (0 + (counts * 4593)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; -- cgit v1.2.3 From 07d11583bb0926ac9fe1cf2f1c7e11c683606a4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 20:21:20 +0200 Subject: Rely on theoretical value, as the closed-loop test with multimeter suggests this is the most accurate measurement --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8c15c66c1..43161aa70 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -751,7 +751,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (0 + (counts * 4593)) / 1000; + unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; -- cgit v1.2.3 From ed9f9ec8646101934041fd1fe0d29ca955aa52c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c5..adca0e88f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; -- cgit v1.2.3 From 0c0b1a4c666671e964d295fe6aea64d7b6a98fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e25851c7d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f..d51120462 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34d..528c8b72a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; -- cgit v1.2.3 From 91becef344833dbb19230ec3f3c69750bce216e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++-------- src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 ++- 4 files changed, 32 insertions(+), 24 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d..0e8f17c77 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { + printf("\tno telem status.\n"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a..454d730d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + hrt_abstime tnow = hrt_absolute_time(); + + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; /* if no radio status messages arrive, lets at least publish that heartbeats were received */ if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; if (_telemetry_status_pub < 0) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3..f4d0c52d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,7 +148,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; int _control_mode_sub; int _hil_frames; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a5..1297c1a9d 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ -- cgit v1.2.3 From 63f1ee184a63ecabd93ce0cfa91afe97fe7e65d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77..7bd2b9754 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { -- cgit v1.2.3 From b344b94d3039273a1659a4b5640e7dc197922767 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 170 +++++++++++++++++++---------------- 1 file changed, 94 insertions(+), 76 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754..cb8c298c0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,7 +81,7 @@ #include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - default: + + default: return; } @@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { /* 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]) - { + 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); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ Mavlink::Mavlink() : _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), - _logbuffer{}, - _total_counter(0), - _receive_thread{}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(10000), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -867,8 +870,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_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)) { + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); @@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_set.target_system == mavlink_system.sysid + && ((mavlink_param_set.target_component == mavlink_system.compid) + || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_request_read.target_system == mavlink_system.sysid + && ((mavlink_param_request_read.target_component == mavlink_system.compid) + || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ @@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string) /* Map severity */ switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; } mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); @@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1475,7 +1487,8 @@ 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, (double)_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); @@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[]) bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { printf("\tno telem status.\n"); } -- cgit v1.2.3 From ef363edfcd810e3b153609f0a723f53db0cd3885 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c0..0c6f8c42f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ instance->count_txerr(); + instance->count_txerrbytes(desired); return; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ret != desired) { instance->count_txerr(); + instance->count_txerrbytes(desired); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; + instance->count_txbytes(desired); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } + perf_end(_loop_perf); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462..70d13acb0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ public: */ void count_txerr(); + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** * Get the receive status of this MAVLink link */ @@ -293,6 +308,14 @@ private: bool _flow_control_enabled; + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d8..54c412ce7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } -- cgit v1.2.3 From 4722b609ccfa35c438c1ef74caba2793c9e04225 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:03:04 +0200 Subject: mavlink: parameters handling moved to MavlinkParametersManager class --- src/modules/mavlink/mavlink_parameters.cpp | 197 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.h | 114 +++++++++++++++++ 2 files changed, 311 insertions(+) create mode 100644 src/modules/mavlink/mavlink_parameters.cpp create mode 100644 src/modules/mavlink/mavlink_parameters.h (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 000000000..b1dae918c --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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 mavlink_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin + */ + +//#include + +#include "mavlink_parameters.h" + +explicit +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index() >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..9b7a04a73 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 + * 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 mavlink_parameters.h + * Mavlink parameters manager definition. + * + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_stream.h" + +class MavlinkParametersManager : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void start_send_all(); + +private: + int _send_all_index; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); + + void subscribe() {} + + void send(const hrt_abstime t); + + void send_param(param_t param); +}; -- cgit v1.2.3 From 344a34bb725a65769a227157f41363fd5d180a14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:03:37 +0200 Subject: mavlink: MavlinkStream API updated --- src/modules/mavlink/mavlink_stream.cpp | 5 +++-- src/modules/mavlink/mavlink_stream.h | 13 ++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 79dc23cc6..da3a55172 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,11 +71,12 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval * _mavlink->get_rate_mult(); - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + _last_sent = t; return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 7d3afbbb5..646931c07 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,8 +46,6 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { @@ -70,7 +68,6 @@ public: * @return the inveral in microseconds (us) between messages */ unsigned get_interval() { return _interval; } - void set_channel(mavlink_channel_t channel); /** * @return 0 if updated / sent, -1 if unchanged @@ -82,6 +79,16 @@ public: virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: Mavlink * _mavlink; unsigned int _interval; -- cgit v1.2.3 From a5f2d1b06645d4db33751aa8392fcbaf7f0856cc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:11:49 +0200 Subject: mavlink: new message sending API, includes fixed --- src/modules/mavlink/mavlink_ftp.h | 1 + src/modules/mavlink/mavlink_main.cpp | 306 +-- src/modules/mavlink/mavlink_main.h | 58 +- src/modules/mavlink/mavlink_messages.cpp | 3186 ++++++++++++++-------------- src/modules/mavlink/mavlink_messages.h | 4 +- src/modules/mavlink/mavlink_mission.cpp | 26 +- src/modules/mavlink/mavlink_parameters.cpp | 6 +- src/modules/mavlink/mavlink_parameters.h | 3 + src/modules/mavlink/module.mk | 1 + 9 files changed, 1715 insertions(+), 1876 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 59237a4c4..1bd1158fb 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -55,6 +55,7 @@ #include #include "mavlink_messages.h" +#include "mavlink_main.h" class MavlinkFTP { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e3c452fb2..dca76c950 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr; static struct file_operations fops; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; /** * mavlink app start / stop handling function @@ -130,6 +131,7 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_manager(nullptr), + _parameters_manager(nullptr), _mission_pub(-1), _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), @@ -145,6 +147,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), + _rate_mult(0.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -bool -Mavlink::check_can_send(const int prio, const unsigned packet_len) +void +Mavlink::send_message(const uint8_t msgid, const void *msg) { + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full */ int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() - && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) { - + if (get_flow_control_enabled() && buf_free == 0) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write @@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const unsigned packet_len) } } - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (should_transmit()) { - _last_write_try_time = hrt_absolute_time(); + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - /* check if there is space in the buffer, let it overflow else */ - ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + _last_write_try_time = hrt_absolute_time(); - if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { - warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); - /* we don't want to send anything just in half, so return */ - count_txerr(); - count_txerrbytes(packet_len); - return true; - } + /* check if there is space in the buffer, let it overflow else */ + if (buf_free >= TX_BUFFER_GAP) { + warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + return; } - return false; -} -void -Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) -{ uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint8_t payload_len = mavlink_message_lengths[msgid]; - unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - if (!check_can_send(prio, packet_len)) { - return; - } - /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; @@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); #endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* send message to UART */ ssize_t ret = write(_uart_fd, buf, packet_len); - if (ret != packet_len) { + if (ret != (int) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -783,7 +777,7 @@ Mavlink::handle_message(const mavlink_message_t *msg) _mission_manager->handle_message(msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(_channel, msg); + _parameters_manager->handle_message(msg); if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ @@ -791,163 +785,6 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int -Mavlink::mavlink_pm_queued_send() -{ - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; -} - -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } - - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ - - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; - } - - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - send_message(&tx_msg); - return OK; -} - -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_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(); - send_statustext_info("[pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid - && ((mavlink_param_set.target_component == mavlink_system.compid) - || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - send_statustext_info(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid - && ((mavlink_param_request_read.target_component == mavlink_system.compid) - || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } -} - int Mavlink::send_statustext_info(const char *string) { @@ -1031,11 +868,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1066,10 +909,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); + stream->subscribe(); LL_APPEND(_streams, stream); return OK; @@ -1267,7 +1109,26 @@ Mavlink::pass_message(const mavlink_message_t *msg) float Mavlink::get_rate_mult() { - return _datarate / 1000.0f; + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); } int @@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } + update_rate_mult(); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = get_rate_mult(); + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 8.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("NAMED_VALUE_FLOAT", 1.0f); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f); + configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); break; @@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + float base_rate_mult = _datarate / 1000.0f; + + MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { + _mission_manager->eventloop(); - /* only send messages if they fit the buffer */ - if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - mavlink_pm_queued_send(); - } - - if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - _mission_manager->eventloop(); - } - - if (!mavlink_logbuffer_is_empty(&_logbuffer) && - check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); @@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; + delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1c62b03d2..7fcbae72e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -58,7 +58,7 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" #include "mavlink_mission.h" - +#include "mavlink_parameters.h" class Mavlink { @@ -160,11 +160,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - bool check_can_send(const int prio, const unsigned packet_len); - - void send_message(const uint8_t msgid, const void *msg, const int prio); - - void send_message(const mavlink_message_t *msg); + void send_message(const uint8_t msgid, const void *msg); void handle_message(const mavlink_message_t *msg); @@ -285,6 +281,7 @@ private: MavlinkStream *_streams; MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; orb_advert_t _mission_pub; int _mission_result_sub; @@ -305,6 +302,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -352,51 +350,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); - - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); - - /** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_name(const char *name); - - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); - - /** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ - void mavlink_pm_start_queued_send(); - void mavlink_update_system(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + static unsigned int interval_from_rate(float rate); + int configure_stream(const char *stream_name, const float rate); /** @@ -420,6 +379,11 @@ private: void pass_message(const mavlink_message_t *msg); + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33985d2..7dc3ebac1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -76,7 +76,7 @@ #include #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -254,6 +254,15 @@ public: return new MavlinkStreamHeartbeat(mavlink); } + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; + } + private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; @@ -291,6 +300,7 @@ protected: } mavlink_heartbeat_t msg; + msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); @@ -298,7 +308,7 @@ protected: msg.autopilot = mavlink_system.type; msg.mavlink_version = 3; - _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; @@ -311,7 +321,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -321,9 +331,14 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamSysStatus(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -334,13 +349,13 @@ private: MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); } void send(const hrt_abstime t) @@ -348,1591 +363,1594 @@ protected: struct vehicle_status_s status; if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); - } - } -}; - - -class MavlinkStreamHighresIMU : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHighresIMU::get_name_static(); - } - - static const char *get_name_static() - { - return "HIGHRES_IMU"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIGHRES_IMU; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHighresIMU(); - } - -private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; - - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; - - /* do not allow top copying this class */ - MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); - -protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) - {} - - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - - void send(const hrt_abstime t) - { - struct sensor_combined_s sensor; - - if (sensor_sub->update(&sensor_time, &sensor)) { - uint16_t fields_updated = 0; - - if (accel_timestamp != sensor.accelerometer_timestamp) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; - } - - if (gyro_timestamp != sensor.timestamp) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; - } - - if (mag_timestamp != sensor.magnetometer_timestamp) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; - } - - if (baro_timestamp != sensor.baro_timestamp) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; - } - - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); - } - } -}; - - -class MavlinkStreamAttitude : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitude::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitude(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); - - -protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); - } - } -}; - - -class MavlinkStreamAttitudeQuaternion : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeQuaternion::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_QUATERNION"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeQuaternion(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); - -protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); - } - } -}; - - -class MavlinkStreamVFRHUD : public MavlinkStream -{ -public: - - const char *get_name() const - { - return MavlinkStreamVFRHUD::get_name_static(); - } - - static const char *get_name_static() - { - return "VFR_HUD"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VFR_HUD; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamVFRHUD(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; - - /* do not allow top copying this class */ - MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); - -protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - struct vehicle_global_position_s pos; - struct actuator_armed_s armed; - struct actuator_controls_s act; - struct airspeed_s airspeed; - - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); - - if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); - } - } -}; - - -class MavlinkStreamGPSRawInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSRawInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_RAW_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_RAW_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSRawInt(); - } - -private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; - - /* do not allow top copying this class */ - MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); - -protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - gps_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_gps_position_s gps; - - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_used); + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *home_sub; - uint64_t home_time; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); - -protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_global_position_s pos; - struct home_position_s home; - - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionNED::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_NED"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_NED; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); - -protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamViconPositionEstimate::get_name_static(); - } - - static const char *get_name_static() - { - return "VICON_POSITION_ESTIMATE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); - -protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_vicon_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSGlobalOrigin::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_GLOBAL_ORIGIN"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - - /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); - -protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - struct home_position_s home; - - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); - } - } - } -}; - -template -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamServoOutputRaw::get_name_static(); - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - } - - static const char *get_name_static() - { - switch (N) { - case 0: - return "SERVO_OUTPUT_RAW_0"; - - case 1: - return "SERVO_OUTPUT_RAW_1"; - - case 2: - return "SERVO_OUTPUT_RAW_2"; - - case 3: - return "SERVO_OUTPUT_RAW_3"; - } - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(); - } - -private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[N]); - } - - void send(const hrt_abstime t) - { - struct actuator_outputs_s act; - - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHILControls::get_name_static(); - } - - static const char *get_name_static() - { - return "HIL_CONTROLS"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIL_CONTROLS; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); - -protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - struct position_setpoint_triplet_s pos_sp_triplet; - struct actuator_outputs_s act; - - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); - - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* multirotors: set number of rotor outputs depending on type */ - - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (i < n) { - /* scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - } - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - } - - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); - -protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - - void send(const hrt_abstime t) - { - struct position_setpoint_triplet_s pos_sp_triplet; - - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); - -protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_setpoint_s pos_sp; - - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_setpoint_s att_sp; - - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRCChannelsRaw::get_name_static(); - } - - static const char *get_name_static() - { - return "RC_CHANNELS_RAW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_RC_CHANNELS_RAW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; - - /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); - -protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - - void send(const hrt_abstime t) - { - struct rc_input_values rc; - - if (rc_sub->update(&rc_time, &rc)) { - const unsigned port_width = 8; - - // Deprecated message (but still needed for compatibility!) - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); - } - - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamManualControl::get_name_static(); - } - - static const char *get_name_static() - { - return "MANUAL_CONTROL"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_MANUAL_CONTROL; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; - - /* do not allow top copying this class */ - MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); - -protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - - void send(const hrt_abstime t) - { - struct manual_control_setpoint_s manual; - - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); - } - } -}; - - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamOpticalFlow::get_name_static(); - } - - static const char *get_name_static() - { - return "OPTICAL_FLOW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_OPTICAL_FLOW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; - - /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); - -protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - - void send(const hrt_abstime t) - { - struct optical_flow_s flow; - - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeControls::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_CONTROLS"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); - -protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - - void send(const hrt_abstime t) - { - struct actuator_controls_s att_ctrl; - - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamNamedValueFloat::get_name_static(); - } - - static const char *get_name_static() - { - return "NAMED_VALUE_FLOAT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; - - /* do not allow top copying this class */ - MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); - -protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - - void send(const hrt_abstime t) - { - struct debug_key_value_s debug; - - if (debug_sub->update(&debug_time, &debug)) { - /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamCameraCapture::get_name_static(); - } - - static const char *get_name_static() - { - return "CAMERA_CAPTURE"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - - /* do not allow top copying this class */ - MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); - -protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), - status_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - (void)status_sub->update(&status); - - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } - } -}; - -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamDistanceSensor::get_name_static(); - } - - static const char *get_name_static() - { - return "DISTANCE_SENSOR"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_DISTANCE_SENSOR; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; - - /* do not allow top copying this class */ - MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); - -protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - - void send(const hrt_abstime t) - { - struct range_finder_report range; - - if (range_sub->update(&range_time, &range)) { - - uint8_t type; - - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); - } - } -}; +//class MavlinkStreamHighresIMU : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHighresIMU::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIGHRES_IMU"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIGHRES_IMU; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHighresIMU(); +// } +// +//private: +// MavlinkOrbSubscription *sensor_sub; +// uint64_t sensor_time; +// +// uint64_t accel_timestamp; +// uint64_t gyro_timestamp; +// uint64_t mag_timestamp; +// uint64_t baro_timestamp; +// +// /* do not allow top copying this class */ +// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); +// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); +// +//protected: +// explicit MavlinkStreamHighresIMU() : MavlinkStream(), +// sensor_sub(nullptr), +// sensor_time(0), +// accel_timestamp(0), +// gyro_timestamp(0), +// mag_timestamp(0), +// baro_timestamp(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); +// } +// +// void send(const hrt_abstime t) +// { +// struct sensor_combined_s sensor; +// +// if (sensor_sub->update(&sensor_time, &sensor)) { +// uint16_t fields_updated = 0; +// +// if (accel_timestamp != sensor.accelerometer_timestamp) { +// /* mark first three dimensions as changed */ +// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// accel_timestamp = sensor.accelerometer_timestamp; +// } +// +// if (gyro_timestamp != sensor.timestamp) { +// /* mark second group dimensions as changed */ +// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// gyro_timestamp = sensor.timestamp; +// } +// +// if (mag_timestamp != sensor.magnetometer_timestamp) { +// /* mark third group dimensions as changed */ +// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// mag_timestamp = sensor.magnetometer_timestamp; +// } +// +// if (baro_timestamp != sensor.baro_timestamp) { +// /* mark last group dimensions as changed */ +// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// baro_timestamp = sensor.baro_timestamp; +// } +// +// mavlink_msg_highres_imu_send(_channel, +// sensor.timestamp, +// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], +// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], +// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], +// sensor.baro_pres_mbar, sensor.differential_pressure_pa, +// sensor.baro_alt_meter, sensor.baro_temp_celcius, +// fields_updated); +// } +// } +//}; +// +// +//class MavlinkStreamAttitude : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitude::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitude(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitude(MavlinkStreamAttitude &); +// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); +// +// +//protected: +// explicit MavlinkStreamAttitude() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_send(_channel, +// att.timestamp / 1000, +// att.roll, att.pitch, att.yaw, +// att.rollspeed, att.pitchspeed, att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamAttitudeQuaternion : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeQuaternion::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_QUATERNION"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeQuaternion(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); +// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); +// +//protected: +// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_quaternion_send(_channel, +// att.timestamp / 1000, +// att.q[0], +// att.q[1], +// att.q[2], +// att.q[3], +// att.rollspeed, +// att.pitchspeed, +// att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamVFRHUD : public MavlinkStream +//{ +//public: +// +// const char *get_name() const +// { +// return MavlinkStreamVFRHUD::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VFR_HUD"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VFR_HUD; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamVFRHUD(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *armed_sub; +// uint64_t armed_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// MavlinkOrbSubscription *airspeed_sub; +// uint64_t airspeed_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); +// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); +// +//protected: +// explicit MavlinkStreamVFRHUD() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0), +// pos_sub(nullptr), +// pos_time(0), +// armed_sub(nullptr), +// armed_time(0), +// act_sub(nullptr), +// act_time(0), +// airspeed_sub(nullptr), +// airspeed_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); +// airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// struct vehicle_global_position_s pos; +// struct actuator_armed_s armed; +// struct actuator_controls_s act; +// struct airspeed_s airspeed; +// +// bool updated = att_sub->update(&att_time, &att); +// updated |= pos_sub->update(&pos_time, &pos); +// updated |= armed_sub->update(&armed_time, &armed); +// updated |= act_sub->update(&act_time, &act); +// updated |= airspeed_sub->update(&airspeed_time, &airspeed); +// +// if (updated) { +// float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); +// uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; +// float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; +// +// mavlink_msg_vfr_hud_send(_channel, +// airspeed.true_airspeed_m_s, +// groundspeed, +// heading, +// throttle, +// pos.alt, +// -pos.vel_d); +// } +// } +//}; +// +// +//class MavlinkStreamGPSRawInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSRawInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_RAW_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_RAW_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSRawInt(); +// } +// +//private: +// MavlinkOrbSubscription *gps_sub; +// uint64_t gps_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); +// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); +// +//protected: +// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), +// gps_sub(nullptr), +// gps_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_gps_position_s gps; +// +// if (gps_sub->update(&gps_time, &gps)) { +// mavlink_msg_gps_raw_int_send(_channel, +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph), +// cm_uint16_from_m_float(gps.epv), +// gps.vel_m_s * 100.0f, +// _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, +// gps.satellites_used); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *home_sub; +// uint64_t home_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); +// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0), +// home_sub(nullptr), +// home_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_global_position_s pos; +// struct home_position_s home; +// +// bool updated = pos_sub->update(&pos_time, &pos); +// updated |= home_sub->update(&home_time, &home); +// +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos.timestamp / 1000, +// pos.lat * 1e7, +// pos.lon * 1e7, +// pos.alt * 1000.0f, +// (pos.alt - home.alt) * 1000.0f, +// pos.vel_n * 100.0f, +// pos.vel_e * 100.0f, +// pos.vel_d * 100.0f, +// _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionNED : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionNED::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_NED"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_NED; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); +// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); +// +//protected: +// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.vx, +// pos.vy, +// pos.vz); +// } +// } +//}; +// +// +// +//class MavlinkStreamViconPositionEstimate : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamViconPositionEstimate::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VICON_POSITION_ESTIMATE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); +// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); +// +//protected: +// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_vicon_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.roll, +// pos.pitch, +// pos.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSGlobalOrigin::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } +// +//private: +// MavlinkOrbSubscription *home_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); +// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); +// +//protected: +// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), +// home_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// struct home_position_s home; +// +// if (home_sub->update(&home)) { +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home.lat * 1e7), +// (int32_t)(home.lon * 1e7), +// (int32_t)(home.alt) * 1000.0f); +// } +// } +// } +//}; +// +//template +//class MavlinkStreamServoOutputRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamServoOutputRaw::get_name_static(); +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +// } +// +// static const char *get_name_static() +// { +// switch (N) { +// case 0: +// return "SERVO_OUTPUT_RAW_0"; +// +// case 1: +// return "SERVO_OUTPUT_RAW_1"; +// +// case 2: +// return "SERVO_OUTPUT_RAW_2"; +// +// case 3: +// return "SERVO_OUTPUT_RAW_3"; +// } +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(); +// } +// +//private: +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); +// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); +// +//protected: +// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_outputs_s act; +// +// if (act_sub->update(&act_time, &act)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act.timestamp / 1000, +// N, +// act.output[0], +// act.output[1], +// act.output[2], +// act.output[3], +// act.output[4], +// act.output[5], +// act.output[6], +// act.output[7]); +// } +// } +//}; +// +// +//class MavlinkStreamHILControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHILControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIL_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIL_CONTROLS; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// uint64_t status_time; +// +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// uint64_t pos_sp_triplet_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamHILControls(MavlinkStreamHILControls &); +// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); +// +//protected: +// explicit MavlinkStreamHILControls() : MavlinkStream(), +// status_sub(nullptr), +// status_time(0), +// pos_sp_triplet_sub(nullptr), +// pos_sp_triplet_time(0), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// struct position_setpoint_triplet_s pos_sp_triplet; +// struct actuator_outputs_s act; +// +// bool updated = act_sub->update(&act_time, &act); +// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); +// updated |= status_sub->update(&status_time, &status); +// +// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state; +// uint8_t mavlink_base_mode; +// uint32_t mavlink_custom_mode; +// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// float out[8]; +// +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; +// +// /* scale outputs depending on system type */ +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* multirotors: set number of rotor outputs depending on type */ +// +// unsigned n; +// +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; +// +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; +// +// default: +// n = 8; +// break; +// } +// +// for (unsigned i = 0; i < 8; i++) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// if (i < n) { +// /* scale PWM out 900..2100 us to 0..1 for rotors */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// +// } else { +// /* scale PWM out 900..2100 us to -1..1 for other channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// } +// +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } +// } +// +// } else { +// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ +// +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale PWM out 900..2100 us to -1..1 for normal channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// +// } else { +// /* scale PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } +// +// } +// } +// +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); +// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// pos_sp_triplet_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// } +// +// void send(const hrt_abstime t) +// { +// struct position_setpoint_triplet_s pos_sp_triplet; +// +// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet.current.lat * 1e7), +// (int32_t)(pos_sp_triplet.current.lon * 1e7), +// (int32_t)(pos_sp_triplet.current.alt * 1000), +// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_sub; +// uint64_t pos_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); +// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); +// +//protected: +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// pos_sp_sub(nullptr), +// pos_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_setpoint_s pos_sp; +// +// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp.x, +// pos_sp.y, +// pos_sp.z, +// pos_sp.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_sp_sub; +// uint64_t att_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); +// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// att_sp_sub(nullptr), +// att_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_setpoint_s att_sp; +// +// if (att_sp_sub->update(&att_sp_time, &att_sp)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// uint64_t att_rates_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// att_rates_sp_sub(nullptr), +// att_rates_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_rates_setpoint_s att_rates_sp; +// +// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp.timestamp / 1000, +// att_rates_sp.roll, +// att_rates_sp.pitch, +// att_rates_sp.yaw, +// att_rates_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRCChannelsRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRCChannelsRaw::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "RC_CHANNELS_RAW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_RC_CHANNELS_RAW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } +// +//private: +// MavlinkOrbSubscription *rc_sub; +// uint64_t rc_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); +// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); +// +//protected: +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// rc_sub(nullptr), +// rc_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// } +// +// void send(const hrt_abstime t) +// { +// struct rc_input_values rc; +// +// if (rc_sub->update(&rc_time, &rc)) { +// const unsigned port_width = 8; +// +// // Deprecated message (but still needed for compatibility!) +// for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc.timestamp_publication / 1000, +// i, +// (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, +// rc.rssi); +// } +// +// // New message +// mavlink_msg_rc_channels_send(_channel, +// rc.timestamp_publication / 1000, +// rc.channel_count, +// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), +// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), +// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), +// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), +// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), +// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), +// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), +// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), +// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), +// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), +// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), +// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), +// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), +// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), +// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), +// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), +// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), +// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), +// rc.rssi); +// } +// } +//}; +// +// +//class MavlinkStreamManualControl : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamManualControl::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "MANUAL_CONTROL"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_MANUAL_CONTROL; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } +// +//private: +// MavlinkOrbSubscription *manual_sub; +// uint64_t manual_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamManualControl(MavlinkStreamManualControl &); +// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); +// +//protected: +// explicit MavlinkStreamManualControl() : MavlinkStream(), +// manual_sub(nullptr), +// manual_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct manual_control_setpoint_s manual; +// +// if (manual_sub->update(&manual_time, &manual)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual.x * 1000, +// manual.y * 1000, +// manual.z * 1000, +// manual.r * 1000, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamOpticalFlow : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamOpticalFlow::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "OPTICAL_FLOW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_OPTICAL_FLOW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } +// +//private: +// MavlinkOrbSubscription *flow_sub; +// uint64_t flow_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); +// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); +// +//protected: +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// flow_sub(nullptr), +// flow_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// } +// +// void send(const hrt_abstime t) +// { +// struct optical_flow_s flow; +// +// if (flow_sub->update(&flow_time, &flow)) { +// mavlink_msg_optical_flow_send(_channel, +// flow.timestamp, +// flow.sensor_id, +// flow.flow_raw_x, flow.flow_raw_y, +// flow.flow_comp_x_m, flow.flow_comp_y_m, +// flow.quality, +// flow.ground_distance_m); +// } +// } +//}; +// +//class MavlinkStreamAttitudeControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } +// +//private: +// MavlinkOrbSubscription *att_ctrl_sub; +// uint64_t att_ctrl_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); +// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); +// +//protected: +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// att_ctrl_sub(nullptr), +// att_ctrl_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_controls_s att_ctrl; +// +// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "rll ctrl ", +// att_ctrl.control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "ptch ctrl ", +// att_ctrl.control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "yaw ctrl ", +// att_ctrl.control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "thr ctrl ", +// att_ctrl.control[3]); +// } +// } +//}; +// +//class MavlinkStreamNamedValueFloat : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamNamedValueFloat::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "NAMED_VALUE_FLOAT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } +// +//private: +// MavlinkOrbSubscription *debug_sub; +// uint64_t debug_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); +// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); +// +//protected: +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// debug_sub(nullptr), +// debug_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// } +// +// void send(const hrt_abstime t) +// { +// struct debug_key_value_s debug; +// +// if (debug_sub->update(&debug_time, &debug)) { +// /* enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(_channel, +// debug.timestamp_ms, +// debug.key, +// debug.value); +// } +// } +//}; +// +//class MavlinkStreamCameraCapture : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamCameraCapture::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "CAMERA_CAPTURE"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); +// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); +// +//protected: +// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// status_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// (void)status_sub->update(&status); +// +// if (status.arming_state == ARMING_STATE_ARMED +// || status.arming_state == ARMING_STATE_ARMED_ERROR) { +// +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); +// +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +//}; +// +//class MavlinkStreamDistanceSensor : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamDistanceSensor::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "DISTANCE_SENSOR"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_DISTANCE_SENSOR; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } +// +//private: +// MavlinkOrbSubscription *range_sub; +// uint64_t range_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); +// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); +// +//protected: +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// range_sub(nullptr), +// range_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// } +// +// void send(const hrt_abstime t) +// { +// struct range_finder_report range; +// +// if (range_sub->update(&range_time, &range)) { +// +// uint8_t type; +// +// switch (range.type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } +// +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; +// +// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, +// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); +// } +// } +//}; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), - new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), - new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), - new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), - new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), - new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), - new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), - new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), - new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), - new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), +// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), +// new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), +// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), +// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), +// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), +// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), +// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), +// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), +// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), +// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), +// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), +// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e42..7e4416609 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 068774c47..d17ccc6f9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -157,15 +157,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) { - mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -175,13 +173,11 @@ void MavlinkMissionManager::send_mission_current(uint16_t seq) { if (seq < _count) { - mavlink_message_t msg; mavlink_mission_current_t wpc; wpc.seq = seq; - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -199,15 +195,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = sysid; wpc.target_component = compid; wpc.count = _count; - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -226,13 +220,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mavlink_mission_item_t wp; format_mavlink_mission_item(&mission_item, &wp); - mavlink_message_t msg; wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -251,13 +244,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (seq < _max_count) { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -272,13 +264,11 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 void MavlinkMissionManager::send_mission_item_reached(uint16_t seq) { - mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b1dae918c..4e8cd4ba4 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -38,11 +38,11 @@ * @author Anton Babushkin */ -//#include +#include #include "mavlink_parameters.h" +#include "mavlink_main.h" -explicit MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1) { @@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt unsigned MavlinkParametersManager::get_size() { - if (_send_all_index() >= 0) { + if (_send_all_index >= 0) { return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } else { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 9b7a04a73..2a5a205e9 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -40,6 +40,9 @@ #pragma once +#include + +#include "mavlink_bridge_header.h" #include "mavlink_stream.h" class MavlinkParametersManager : public MavlinkStream diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1986ae3c8..08faf102a 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,6 +40,7 @@ SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ + mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ -- cgit v1.2.3 From d70b21c51aacae1a3dae755dca4ba9c3fa7a0d88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 15:37:56 +0200 Subject: mavlink: move commands stream to mavlink_messages.cpp, bugs fixed --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- src/modules/mavlink/mavlink_commands.cpp | 73 ----------------------------- src/modules/mavlink/mavlink_commands.h | 65 ------------------------- src/modules/mavlink/mavlink_main.cpp | 14 +++--- src/modules/mavlink/mavlink_messages.cpp | 70 +++++++++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.cpp | 2 +- src/modules/mavlink/module.mk | 1 - 7 files changed, 78 insertions(+), 149 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_commands.cpp delete mode 100644 src/modules/mavlink/mavlink_commands.h (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c..d82c2dae7 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp deleted file mode 100644 index b502c3c86..000000000 --- a/src/modules/mavlink/mavlink_commands.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * 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 mavlink_commands.cpp - * Mavlink commands stream implementation. - * - * @author Anton Babushkin - */ - -#include "mavlink_commands.h" - -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : - _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), - _cmd{}, - _channel(channel), - _cmd_time(0) -{ -} - -void -MavlinkCommandsStream::update(const hrt_abstime t) -{ - struct vehicle_command_s cmd; - - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } - } -} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h deleted file mode 100644 index abdba34b9..000000000 --- a/src/modules/mavlink/mavlink_commands.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * 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 mavlink_commands.h - * Mavlink commands stream definition. - * - * @author Anton Babushkin - */ - -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ - -#include -#include - -class Mavlink; -class MavlinkCommansStream; - -#include "mavlink_main.h" - -class MavlinkCommandsStream -{ -private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; - -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; - -#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dca76c950..4f4bf8691 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -78,7 +78,6 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems @@ -840,7 +839,7 @@ Mavlink::send_statustext(unsigned severity, const char *string) break; } - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); + send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); return OK; } else { @@ -1313,13 +1312,14 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode */ /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); _parameters_manager->set_interval(interval_from_rate(30.0f)); @@ -1384,9 +1384,6 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1469,7 +1466,8 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + // TODO implement message resending + //_mavlink_resend_uart(_channel, &msg); } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7dc3ebac1..a8b4d11cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -312,6 +312,75 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { return 0; } // commands stream is not regular and not predictable + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(nullptr), + _cmd_time(0) + {} + + void subscribe() { + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + } + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t mavcmd; + + mavcmd.target_system = cmd.target_system; + mavcmd.target_component = cmd.target_component; + mavcmd.command = cmd.command; + mavcmd.confirmation = cmd.confirmation; + mavcmd.param1 = cmd.param1; + mavcmd.param2 = cmd.param2; + mavcmd.param3 = cmd.param3; + mavcmd.param4 = cmd.param4; + mavcmd.param5 = cmd.param5; + mavcmd.param6 = cmd.param6; + mavcmd.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -1926,6 +1995,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), // new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 4e8cd4ba4..cd5f53d88 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -147,7 +147,7 @@ MavlinkParametersManager::send(const hrt_abstime t) if (_send_all_index >= 0) { send_param(param_for_index(_send_all_index)); _send_all_index++; - if (_send_all_index >= param_count()) { + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 08faf102a..91fdd6154 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -45,7 +45,6 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From 2313bf7cd5cbf14ffaec61116cce84b9c0c18653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:59:04 +0200 Subject: mTECS: Note better filter defaults to get airspeed response a bit smoother. Damp also ACC response with filter more than currently --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe20..7cfe63fbc 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); /** * P gain for the airspeed control @@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); /** * Minimal acceleration (air) @@ -286,13 +293,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); */ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); -/** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * -- cgit v1.2.3 From 7ecf66c06d15fb9a8c04f96b5bd05fe1c93138fe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 17:36:10 +0200 Subject: mavlink: bugs fixed --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++------------ src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/mavlink/mavlink_stream.cpp | 13 +++++++++++-- 3 files changed, 23 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4f4bf8691..f3246c380 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -146,7 +146,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), - _rate_mult(0.0f), + _rate_mult(1.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -724,7 +724,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free >= TX_BUFFER_GAP) { + if (buf_free < TX_BUFFER_GAP) { warnx("SKIP msgid %i, %i bytes", msgid, packet_len); /* no enough space in buffer to send */ count_txerr(); @@ -749,9 +749,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) uint16_t checksum; crc_init(&checksum); crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); -#if MAVLINK_CRC_EXTRA crc_accumulate(mavlink_message_crcs[msgid], &checksum); -#endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); @@ -1127,7 +1125,7 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); + _rate_mult = ((float)_datarate - const_rate) / rate; } int @@ -1250,8 +1248,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - update_rate_mult(); - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1328,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1340,6 +1336,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + */ break; case MAVLINK_MODE_CAMERA: @@ -1353,9 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - float base_rate_mult = _datarate / 1000.0f; MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); @@ -1374,6 +1368,9 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + warnx("rate mult %f", (double)_rate_mult); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -1410,7 +1407,8 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - _mission_manager->eventloop(); + // TODO missions + //_mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8b4d11cc..20b1a966f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -305,7 +305,7 @@ protected: msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); msg.type = mavlink_system.type; - msg.autopilot = mavlink_system.type; + msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); @@ -352,7 +352,7 @@ protected: {} void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index da3a55172..5b9e45d3e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,12 +71,21 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; - unsigned int interval = _interval * _mavlink->get_rate_mult(); + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = t; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } -- cgit v1.2.3 From 20698c751c62ca6f11aa910b3c3f180fe30211ff Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:40:55 +0200 Subject: mavlink: HIGHRES_IMU stream added --- src/modules/mavlink/mavlink_main.cpp | 5 +- src/modules/mavlink/mavlink_messages.cpp | 246 ++++++++++++++++--------------- 2 files changed, 133 insertions(+), 118 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f3246c380..e49e99a9b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1324,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1336,7 +1336,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - */ break; case MAVLINK_MODE_CAMERA: @@ -1369,7 +1368,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %f", (double)_rate_mult); + warnx("rate mult %.2f", (double)_rate_mult); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 20b1a966f..a77d34c71 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -454,99 +454,115 @@ protected: }; -//class MavlinkStreamHighresIMU : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamHighresIMU::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "HIGHRES_IMU"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_HIGHRES_IMU; -// } -// -// static MavlinkStream *new_instance() -// { -// return new MavlinkStreamHighresIMU(); -// } -// -//private: -// MavlinkOrbSubscription *sensor_sub; -// uint64_t sensor_time; -// -// uint64_t accel_timestamp; -// uint64_t gyro_timestamp; -// uint64_t mag_timestamp; -// uint64_t baro_timestamp; -// -// /* do not allow top copying this class */ -// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); -// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); -// -//protected: -// explicit MavlinkStreamHighresIMU() : MavlinkStream(), -// sensor_sub(nullptr), -// sensor_time(0), -// accel_timestamp(0), -// gyro_timestamp(0), -// mag_timestamp(0), -// baro_timestamp(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); -// } -// -// void send(const hrt_abstime t) -// { -// struct sensor_combined_s sensor; -// -// if (sensor_sub->update(&sensor_time, &sensor)) { -// uint16_t fields_updated = 0; -// -// if (accel_timestamp != sensor.accelerometer_timestamp) { -// /* mark first three dimensions as changed */ -// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// accel_timestamp = sensor.accelerometer_timestamp; -// } -// -// if (gyro_timestamp != sensor.timestamp) { -// /* mark second group dimensions as changed */ -// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// gyro_timestamp = sensor.timestamp; -// } -// -// if (mag_timestamp != sensor.magnetometer_timestamp) { -// /* mark third group dimensions as changed */ -// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// mag_timestamp = sensor.magnetometer_timestamp; -// } -// -// if (baro_timestamp != sensor.baro_timestamp) { -// /* mark last group dimensions as changed */ -// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// baro_timestamp = sensor.baro_timestamp; -// } -// -// mavlink_msg_highres_imu_send(_channel, -// sensor.timestamp, -// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], -// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], -// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], -// sensor.baro_pres_mbar, sensor.differential_pressure_pa, -// sensor.baro_alt_meter, sensor.baro_temp_celcius, -// fields_updated); -// } -// } -//}; +class MavlinkStreamHighresIMU : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHighresIMU::get_name_static(); + } + + static const char *get_name_static() + { + return "HIGHRES_IMU"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; + + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; + + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + +protected: + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + sensor_sub(nullptr), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} + + void subscribe() + { + sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); + } + + void send(const hrt_abstime t) + { + struct sensor_combined_s sensor; + + if (sensor_sub->update(&sensor_time, &sensor)) { + uint16_t fields_updated = 0; + + if (accel_timestamp != sensor.accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor.accelerometer_timestamp; + } + + if (gyro_timestamp != sensor.timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor.timestamp; + } + + if (mag_timestamp != sensor.magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor.magnetometer_timestamp; + } + + if (baro_timestamp != sensor.baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor.baro_timestamp; + } + + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); + } + } +}; // // //class MavlinkStreamAttitude : public MavlinkStream @@ -567,7 +583,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitude(); // } @@ -624,7 +640,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeQuaternion(); // } @@ -686,7 +702,7 @@ protected: // return MAVLINK_MSG_ID_VFR_HUD; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamVFRHUD(); // } @@ -783,7 +799,7 @@ protected: // return MAVLINK_MSG_ID_GPS_RAW_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSRawInt(); // } @@ -846,7 +862,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionInt(); // } @@ -918,7 +934,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_NED; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionNED(); // } @@ -979,7 +995,7 @@ protected: // return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamViconPositionEstimate(); // } @@ -1039,7 +1055,7 @@ protected: // return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSGlobalOrigin(); // } @@ -1109,7 +1125,7 @@ protected: // } // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamServoOutputRaw(); // } @@ -1179,7 +1195,7 @@ protected: // return MAVLINK_MSG_ID_HIL_CONTROLS; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamHILControls(); // } @@ -1319,7 +1335,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionSetpointInt(); // } @@ -1375,7 +1391,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionSetpoint(); // } @@ -1433,7 +1449,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawThrustSetpoint(); // } @@ -1491,7 +1507,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); // } @@ -1549,7 +1565,7 @@ protected: // return MAVLINK_MSG_ID_RC_CHANNELS_RAW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRCChannelsRaw(); // } @@ -1643,7 +1659,7 @@ protected: // return MAVLINK_MSG_ID_MANUAL_CONTROL; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamManualControl(); // } @@ -1702,7 +1718,7 @@ protected: // return MAVLINK_MSG_ID_OPTICAL_FLOW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamOpticalFlow(); // } @@ -1760,7 +1776,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeControls(); // } @@ -1828,7 +1844,7 @@ protected: // return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamNamedValueFloat(); // } @@ -1886,7 +1902,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamCameraCapture(); // } @@ -1944,7 +1960,7 @@ protected: // return MAVLINK_MSG_ID_DISTANCE_SENSOR; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamDistanceSensor(); // } @@ -1997,7 +2013,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), -// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), // new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), // new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), -- cgit v1.2.3 From a65b7aee0bc07eeb7545f4a3206e860791b64a36 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:54:48 +0200 Subject: mavlink: ATTITUDE, ATTITUDE_QUATERNION, VFR_HUD streams added --- src/modules/mavlink/mavlink_messages.cpp | 544 ++++++++++++++++--------------- 1 file changed, 284 insertions(+), 260 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a77d34c71..575268814 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -563,224 +563,248 @@ protected: } } }; -// -// -//class MavlinkStreamAttitude : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitude::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ATTITUDE; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitude(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitude(MavlinkStreamAttitude &); -// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); -// -// -//protected: -// explicit MavlinkStreamAttitude() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// -// if (att_sub->update(&att_time, &att)) { -// mavlink_msg_attitude_send(_channel, -// att.timestamp / 1000, -// att.roll, att.pitch, att.yaw, -// att.rollspeed, att.pitchspeed, att.yawspeed); -// } -// } -//}; -// -// -//class MavlinkStreamAttitudeQuaternion : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitudeQuaternion::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE_QUATERNION"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitudeQuaternion(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); -// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); -// -//protected: -// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// -// if (att_sub->update(&att_time, &att)) { -// mavlink_msg_attitude_quaternion_send(_channel, -// att.timestamp / 1000, -// att.q[0], -// att.q[1], -// att.q[2], -// att.q[3], -// att.rollspeed, -// att.pitchspeed, -// att.yawspeed); -// } -// } -//}; -// -// -//class MavlinkStreamVFRHUD : public MavlinkStream -//{ -//public: -// -// const char *get_name() const -// { -// return MavlinkStreamVFRHUD::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "VFR_HUD"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_VFR_HUD; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamVFRHUD(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// MavlinkOrbSubscription *armed_sub; -// uint64_t armed_time; -// -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// MavlinkOrbSubscription *airspeed_sub; -// uint64_t airspeed_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); -// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); -// -//protected: -// explicit MavlinkStreamVFRHUD() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0), -// pos_sub(nullptr), -// pos_time(0), -// armed_sub(nullptr), -// armed_time(0), -// act_sub(nullptr), -// act_time(0), -// airspeed_sub(nullptr), -// airspeed_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); -// airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// struct vehicle_global_position_s pos; -// struct actuator_armed_s armed; -// struct actuator_controls_s act; -// struct airspeed_s airspeed; -// -// bool updated = att_sub->update(&att_time, &att); -// updated |= pos_sub->update(&pos_time, &pos); -// updated |= armed_sub->update(&armed_time, &armed); -// updated |= act_sub->update(&act_time, &act); -// updated |= airspeed_sub->update(&airspeed_time, &airspeed); -// -// if (updated) { -// float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); -// uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; -// float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; -// -// mavlink_msg_vfr_hud_send(_channel, -// airspeed.true_airspeed_m_s, -// groundspeed, -// heading, -// throttle, -// pos.alt, -// -pos.vel_d); -// } -// } -//}; -// -// + + +class MavlinkStreamAttitude : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitude::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + +protected: + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); + } + } +}; + + +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeQuaternion::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_QUATERNION"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + +protected: + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); + } + } +}; + + +class MavlinkStreamVFRHUD : public MavlinkStream +{ +public: + + const char *get_name() const + { + return MavlinkStreamVFRHUD::get_name_static(); + } + + static const char *get_name_static() + { + return "VFR_HUD"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + + MavlinkOrbSubscription *armed_sub; + uint64_t armed_time; + + MavlinkOrbSubscription *act_sub; + uint64_t act_time; + + MavlinkOrbSubscription *airspeed_sub; + uint64_t airspeed_time; + + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + +protected: + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0), + pos_sub(nullptr), + pos_time(0), + armed_sub(nullptr), + armed_time(0), + act_sub(nullptr), + act_time(0), + airspeed_sub(nullptr), + airspeed_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + armed_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_armed)); + act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + airspeed_sub = _mavlink->add_orb_subscription(ORB_ID(airspeed)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + struct vehicle_global_position_s pos; + struct actuator_armed_s armed; + struct actuator_controls_s act; + struct airspeed_s airspeed; + + bool updated = att_sub->update(&att_time, &att); + updated |= pos_sub->update(&pos_time, &pos); + updated |= armed_sub->update(&armed_time, &armed); + updated |= act_sub->update(&act_time, &act); + updated |= airspeed_sub->update(&airspeed_time, &airspeed); + + if (updated) { + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = pos.alt; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); + } + } +}; + + //class MavlinkStreamGPSRawInt : public MavlinkStream //{ //public: @@ -818,9 +842,9 @@ protected: // gps_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); // } // // void send(const hrt_abstime t) @@ -886,10 +910,10 @@ protected: // home_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); // } // // void send(const hrt_abstime t) @@ -953,9 +977,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); // } // // void send(const hrt_abstime t) @@ -1014,9 +1038,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); // } // // void send(const hrt_abstime t) @@ -1072,9 +1096,9 @@ protected: // home_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); // } // // void send(const hrt_abstime t) @@ -1144,7 +1168,7 @@ protected: // act_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { // orb_id_t act_topics[] = { // ORB_ID(actuator_outputs_0), @@ -1153,7 +1177,7 @@ protected: // ORB_ID(actuator_outputs_3) // }; // -// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// act_sub = _mavlink->add_orb_subscription(act_topics[N]); // } // // void send(const hrt_abstime t) @@ -1224,11 +1248,11 @@ protected: // act_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); // } // // void send(const hrt_abstime t) @@ -1352,9 +1376,9 @@ protected: // pos_sp_triplet_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); // } // // void send(const hrt_abstime t) @@ -1410,9 +1434,9 @@ protected: // pos_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); // } // // void send(const hrt_abstime t) @@ -1468,9 +1492,9 @@ protected: // att_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); // } // // void send(const hrt_abstime t) @@ -1526,9 +1550,9 @@ protected: // att_rates_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); // } // // void send(const hrt_abstime t) @@ -1584,9 +1608,9 @@ protected: // rc_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc)); // } // // void send(const hrt_abstime t) @@ -1678,9 +1702,9 @@ protected: // manual_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); // } // // void send(const hrt_abstime t) @@ -1737,9 +1761,9 @@ protected: // flow_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); // } // // void send(const hrt_abstime t) @@ -1795,9 +1819,9 @@ protected: // att_ctrl_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); // } // // void send(const hrt_abstime t) @@ -1863,9 +1887,9 @@ protected: // debug_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); // } // // void send(const hrt_abstime t) @@ -1919,9 +1943,9 @@ protected: // status_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); // } // // void send(const hrt_abstime t) @@ -1979,9 +2003,9 @@ protected: // range_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); // } // // void send(const hrt_abstime t) @@ -2014,9 +2038,9 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), -// new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), -// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), -// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), + new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), + new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), // new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), // new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), // new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), -- cgit v1.2.3 From ea2dce39927b7afcdfae79c059cc57342c70904e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 23:10:10 +0200 Subject: mavlink: MavlinkStream API simplifyed --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 171 ++++++++++++------------------- src/modules/mavlink/mavlink_parameters.h | 2 - src/modules/mavlink/mavlink_stream.h | 1 - 4 files changed, 66 insertions(+), 109 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e49e99a9b..0cf708017 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -908,7 +908,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* create new instance */ stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(); LL_APPEND(_streams, stream); return OK; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 575268814..863b7a1d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -264,8 +264,8 @@ public: } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); @@ -273,28 +273,22 @@ private: protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr), - pos_sp_triplet_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } @@ -347,14 +341,10 @@ private: protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), - _cmd_sub(nullptr), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), _cmd_time(0) {} - void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - } - void send(const hrt_abstime t) { struct vehicle_command_s cmd; @@ -411,7 +401,7 @@ public: } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); @@ -419,19 +409,14 @@ private: protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { + if (_status_sub->update(&status)) { mavlink_sys_status_t msg; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; @@ -483,13 +468,13 @@ public: } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); @@ -497,48 +482,43 @@ private: protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe() - { - sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } mavlink_highres_imu_t msg; @@ -594,8 +574,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); @@ -604,20 +584,15 @@ private: protected: explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -663,8 +638,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); @@ -672,20 +647,15 @@ private: protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_quaternion_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -733,20 +703,20 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); @@ -754,27 +724,18 @@ private: protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = _mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -783,11 +744,11 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { mavlink_vfr_hud_t msg; diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 2a5a205e9..5576e6b84 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -109,8 +109,6 @@ private: protected: explicit MavlinkParametersManager(Mavlink *mavlink); - void subscribe() {} - void send(const hrt_abstime t); void send_param(param_t param); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 646931c07..ef22dc443 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -75,7 +75,6 @@ public: int update(const hrt_abstime t); static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; -- cgit v1.2.3 From 9df1da1b7cd140e9452c73c6307befeadd6ce4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:21 +0200 Subject: mavlink: STATUSTEXT implemented via streams API --- src/modules/mavlink/mavlink_main.cpp | 72 +++++--------------- src/modules/mavlink/mavlink_main.h | 18 +++-- src/modules/mavlink/mavlink_messages.cpp | 110 ++++++++++++++++++++++++++----- 3 files changed, 119 insertions(+), 81 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0cf708017..7d3fb97e1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -452,7 +452,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); msg.severity = (unsigned char)cmd; @@ -782,67 +781,32 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int +void Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } -int +void Mavlink::send_statustext_critical(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); } -int +void Mavlink::send_statustext_emergency(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); } -int +void Mavlink::send_statustext(unsigned severity, const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - /* Map severity */ - switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; - } - - send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); - return OK; - - } else { - return ERROR; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1312,6 +1276,9 @@ Mavlink::task_main(int argc, char *argv[]) /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ configure_stream("COMMAND_LONG", 100.0f); @@ -1367,7 +1334,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f", (double)_rate_mult); + warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ @@ -1407,15 +1374,6 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { // TODO missions //_mission_manager->eventloop(); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - send_statustext(msg.severity, msg.text); - } - } } /* pass messages from other UARTs or FTP worker */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7fcbae72e..87bb9d9ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -188,29 +188,33 @@ public: * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_info(const char *string); + void send_statustext_info(const char *string); /** * Send a status text with loglevel CRITICAL * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_critical(const char *string); + void send_statustext_critical(const char *string); /** * Send a status text with loglevel EMERGENCY * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_emergency(const char *string); + void send_statustext_emergency(const char *string); /** - * Send a status text with loglevel + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level */ - int send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -259,6 +263,8 @@ public: */ struct telemetry_status_s& get_rx_status() { return _rstatus; } + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + protected: Mavlink *next; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 863b7a1d4..4333369fe 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include + #include #include - #include #include #include @@ -72,8 +72,8 @@ #include #include #include - #include +#include #include "mavlink_messages.h" #include "mavlink_main.h" @@ -306,6 +306,77 @@ protected: } }; +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + /* map severity */ + switch (logmsg.severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + strncpy(msg.text, logmsg.text, sizeof(msg.text)); + + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + } + } + } +}; + class MavlinkStreamCommandLong : public MavlinkStream { public: @@ -329,7 +400,9 @@ public: return new MavlinkStreamCommandLong(mavlink); } - unsigned get_size() { return 0; } // commands stream is not regular and not predictable + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } private: MavlinkOrbSubscription *_cmd_sub; @@ -352,21 +425,21 @@ protected: if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_command_long_t mavcmd; - - mavcmd.target_system = cmd.target_system; - mavcmd.target_component = cmd.target_component; - mavcmd.command = cmd.command; - mavcmd.confirmation = cmd.confirmation; - mavcmd.param1 = cmd.param1; - mavcmd.param2 = cmd.param2; - mavcmd.param3 = cmd.param3; - mavcmd.param4 = cmd.param4; - mavcmd.param5 = cmd.param5; - mavcmd.param6 = cmd.param6; - mavcmd.param7 = cmd.param7; - - _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + mavlink_command_long_t msg; + + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } } } @@ -1996,6 +2069,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), -- cgit v1.2.3 From eb4015ced1b55a280fe93e10811c0dad5c719438 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:56 +0200 Subject: mavlink: strange FIXME comment removed --- src/modules/mavlink/mavlink_mission.h | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b792b9aaf..5bf80768c 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -46,7 +46,6 @@ #include "mavlink_rate_limiter.h" #include -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, -- cgit v1.2.3 From 897984c4f4a43136320c39f75fef106d3447ce47 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:28:02 +0200 Subject: mavlink: bug fixed, don't delete parameters stream --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7d3fb97e1..44329e0d2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1443,7 +1443,6 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; - delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; -- cgit v1.2.3 From 87334a987a5b571f6cc3d962df1194584de4fd7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:42:45 +0200 Subject: Return 0 for a non-reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 768e0be35..3d504d395 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) ResetHeight(); ResetStoredStates(); - ret = 3; + ret = 0; } // Reset the filter if gyro offsets are excessive -- cgit v1.2.3 From f03f6ddd74b1aca710f4675bbee18608eeea1f08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:57:15 +0200 Subject: Improve user feedback on mission load fails --- src/modules/navigator/mission.cpp | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..12f6b9c21 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -314,30 +314,36 @@ Mission::set_mission_items() /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* get home distance state */ + bool home_dist_ok = check_dist_1wp(); + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = !home_dist_ok; + /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { - /* no mission available, switch to loiter */ + /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: mission finished"); - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: no mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + } else if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; @@ -397,7 +403,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; @@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR waypoint could not be read"); + "ERROR waypoint could not be read"); return false; } @@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP is cycling, giving up"); + "ERROR DO JUMP is cycling, giving up"); return false; } -- cgit v1.2.3 From b4e6f535ea15055f39b122eb87004c97796eb584 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:57:30 +0200 Subject: mavlink: onboard links should only pass on messages from the same system ID --- src/modules/mavlink/mavlink_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c6f8c42f..8cb0152fd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -483,7 +483,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } -- cgit v1.2.3 From 36d8d73aeb7ef214979a64960df727bf3bab048c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:58:17 +0200 Subject: mavlink: use correct component ID --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..92e469619 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || status.arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From 58aa9f28f060f7adf7955b39d72ac972c03f1348 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:59:02 +0200 Subject: mavlink: when detecting spoof, be more verbose --- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 54c412ce7..69e3ef31d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } -- cgit v1.2.3 From b31ddc193c0e3dff3a8e4b4c343848f0f64e61ed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:31:25 +0200 Subject: mavlink: tix mission manager to use MavlinkStream API --- src/modules/mavlink/mavlink_main.cpp | 27 ++---- src/modules/mavlink/mavlink_main.h | 2 - src/modules/mavlink/mavlink_mission.cpp | 25 ++++-- src/modules/mavlink/mavlink_mission.h | 118 +++++++++++++++------------ src/modules/mavlink/mavlink_rate_limiter.cpp | 2 +- 5 files changed, 97 insertions(+), 77 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 44329e0d2..df078325a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -131,8 +131,6 @@ Mavlink::Mavlink() : _streams(nullptr), _mission_manager(nullptr), _parameters_manager(nullptr), - _mission_pub(-1), - _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, @@ -1255,12 +1253,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - - /* create mission manager */ - _mission_manager = new MavlinkMissionManager(this); - _mission_manager->set_verbose(_verbose); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1287,6 +1279,14 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); @@ -1315,12 +1315,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } - float base_rate_mult = _datarate / 1000.0f; - - MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1371,11 +1367,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (fast_rate_limiter.check(t)) { - // TODO missions - //_mission_manager->eventloop(); - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 87bb9d9ad..e3bad828f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -289,8 +289,6 @@ private: MavlinkMissionManager *_mission_manager; MavlinkParametersManager *_parameters_manager; - orb_advert_t _mission_pub; - int _mission_result_sub; MAVLINK_MODE _mode; mavlink_channel_t _channel; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d17ccc6f9..7a761588a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -59,8 +59,7 @@ static const int ERROR = -1; -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : - _mavlink(mavlink), +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(-1), - _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _slow_rate_limiter(_interval / 10.0f), _verbose(false), - _channel(mavlink->get_channel()), _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager() close(_mission_result_sub); } +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void MavlinkMissionManager::init_offboard_mission() { @@ -275,9 +287,10 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void -MavlinkMissionManager::eventloop() +MavlinkMissionManager::send(const hrt_abstime now) { - hrt_abstime now = hrt_absolute_time(); + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 5bf80768c..8ff27f87d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -42,9 +42,11 @@ #pragma once +#include + #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" -#include +#include "mavlink_stream.h" enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, @@ -65,20 +67,75 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -class Mavlink; - -class MavlinkMissionManager { +class MavlinkMissionManager : public MavlinkStream { public: - MavlinkMissionManager(Mavlink *parent); - ~MavlinkMissionManager(); + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + uint8_t _comp_id; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + void init_offboard_mission(); int update_active_mission(int dataman_id, unsigned count, int seq); - void send_message(mavlink_message_t *msg); - /** * @brief Sends an waypoint ack message */ @@ -110,10 +167,6 @@ public: */ void send_mission_item_reached(uint16_t seq); - void eventloop(); - - void handle_message(const mavlink_message_t *msg); - void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); @@ -138,43 +191,8 @@ public: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - void set_verbose(bool v) { _verbose = v; } - -private: - Mavlink* _mavlink; - - enum MAVLINK_WPM_STATES _state; ///< Current state - - uint64_t _time_last_recv; - uint64_t _time_last_sent; - - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximum number of mission items - - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission - - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; - - MavlinkRateLimiter _slow_rate_limiter; - - bool _verbose; - - mavlink_channel_t _channel; - uint8_t _comp_id; +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); - /* do not allow copying this class */ - MavlinkMissionManager(const MavlinkMissionManager&); - MavlinkMissionManager operator=(const MavlinkMissionManager&); + void send(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17..d3b3e1cde 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } -- cgit v1.2.3 From c486aa536f3c0169e1abb0f4c2b60baa1e2b053b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:34:19 +0200 Subject: mavlink: bug fixed, don't delete mission manages twice --- src/modules/mavlink/mavlink_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index df078325a..e27a940cf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1433,8 +1433,6 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } - delete _mission_manager; - delete _subscribe_to_stream; _subscribe_to_stream = nullptr; -- cgit v1.2.3 From fd52e5561eaf14f9c6ca1abc00cb82b0e3d38ec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:46:39 +0200 Subject: update comment about condition_global_position_valid --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b75..cbb148eda 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -186,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; -- cgit v1.2.3 From 8f0af1c5fe5a843c56fff2dc70acb2fc0e7e1b90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 00:16:02 +0200 Subject: mavlink: forwarding and FTP fixed, flow control detector fixed --- src/modules/mavlink/mavlink_main.cpp | 97 ++++++++++++++++++++++++------------ src/modules/mavlink/mavlink_main.h | 19 ++++--- 2 files changed, 77 insertions(+), 39 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e27a940cf..aaee0455a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate -#define TX_BUFFER_GAP 256 +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN static Mavlink *_mavlink_instances = nullptr; @@ -252,25 +252,6 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } -unsigned -Mavlink::get_free_tx_buf() -{ - unsigned buf_free; - - if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.telem_time > 0 && - (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - - return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); - - } else { - return buf_free; - } - } else { - return 0; - } -} - void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -686,15 +667,9 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::send_message(const uint8_t msgid, const void *msg) +unsigned +Mavlink::get_free_tx_buf() { - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full @@ -702,7 +677,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) int buf_free = 0; (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() && buf_free == 0) { + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write @@ -714,6 +689,19 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) enable_flow_control(false); } } + return buf_free; +} + +void +Mavlink::send_message(const uint8_t msgid, const void *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -722,7 +710,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -764,6 +752,52 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) } } +void +Mavlink::resend_message(mavlink_message_t *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); + + _last_write_try_time = hrt_absolute_time(); + + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); + + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -1411,8 +1445,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - // TODO implement message resending - //_mavlink_resend_uart(_channel, &msg); + resend_message(&msg); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e3bad828f..4121e286e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -162,6 +162,11 @@ public: void send_message(const uint8_t msgid, const void *msg); + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -251,13 +256,6 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - /** * Get the receive status of this MAVLink link */ @@ -358,6 +356,13 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); -- cgit v1.2.3 From 80a197f0fffb39037ce271bbc2e2107f2208eb01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 25 Jul 2014 09:45:23 +0200 Subject: Revert "mavlink: use correct component ID" This reverts commit 36d8d73aeb7ef214979a64960df727bf3bab048c. --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 92e469619..6885bebde 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || status.arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From 1938ef16e39ff937a3076489f3eff0a2eb4bbb65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:27:07 +0200 Subject: mavlink: don't scale up rates, debug output removed --- src/modules/mavlink/mavlink_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index aaee0455a..572d5c19b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -610,6 +610,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -651,8 +654,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 150.0f); } /* disable HIL */ @@ -710,7 +712,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -769,7 +770,6 @@ Mavlink::resend_message(mavlink_message_t *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1120,7 +1120,8 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / rate; + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); } int @@ -1364,7 +1365,6 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ -- cgit v1.2.3 From 07c4144cde1026c4d966d043d12e36ba686c0781 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:30:06 +0200 Subject: mavlink: message buffer size fixed --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 572d5c19b..408799be4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1268,7 +1268,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } -- cgit v1.2.3 From 54b9698d6560290e386b8dd2a7b9f0b6f4c5f57a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 17:48:45 +0200 Subject: circuit_breakers: added param to disable airspeed check --- src/modules/commander/commander.cpp | 2 ++ src/modules/commander/state_machine_helper.cpp | 4 +++- src/modules/systemlib/circuit_breaker.c | 12 ++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 5 files changed, 19 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a0c896178..daba4e740 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -736,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -977,6 +978,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..3c3d2f233 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -669,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) goto system_eval; } - if (!status->is_rotary_wing) { + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { /* accel done, close it */ close(fd); fd = orb_subscribe(ORB_ID(airspeed)); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749e..64317a18a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce8..b60584608 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,7 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index cbb148eda..b683bf98a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -226,6 +226,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** -- cgit v1.2.3 From 5d36381dc5f3bbb1eefc70158680f9622ac437b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:38:14 +0200 Subject: EKF: Fix wind publication, fix commented out flags --- .../ekf_att_pos_estimator_main.cpp | 27 ++++------------------ 1 file changed, 4 insertions(+), 23 deletions(-) (limited to 'src/modules') 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 ccc497323..91d17e787 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 @@ -630,10 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); - // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); - // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); - // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); - // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1465,25 +1465,6 @@ FixedwingEstimator::task_main() _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - - } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; -- cgit v1.2.3 From 8e12d79ef4b32da98dfb13af1321a6855ecbdc3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:43:39 +0200 Subject: Increase GPS position timeout to real-life timeouts. More work needed. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daba4e740..6c24734e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -124,7 +124,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 @@ -1111,7 +1111,7 @@ int commander_thread_main(int argc, char *argv[]) bool eph_epv_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { eph_epv_good = false; } else { -- cgit v1.2.3 From e1361fdc02a75d72849b52a0102e02e400eecd9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 00:07:01 +0200 Subject: mavlink: GPS_RAW_INT stream added --- src/modules/mavlink/mavlink_messages.cpp | 134 ++++++++++++++++--------------- 1 file changed, 71 insertions(+), 63 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4333369fe..8f0c15f5a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -839,69 +839,77 @@ protected: }; -//class MavlinkStreamGPSRawInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGPSRawInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GPS_RAW_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GPS_RAW_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGPSRawInt(); -// } -// -//private: -// MavlinkOrbSubscription *gps_sub; -// uint64_t gps_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); -// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); -// -//protected: -// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), -// gps_sub(nullptr), -// gps_time(0) -// {} -// -// void subscribe() -// { -// gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_gps_position_s gps; -// -// if (gps_sub->update(&gps_time, &gps)) { -// mavlink_msg_gps_raw_int_send(_channel, -// gps.timestamp_position, -// gps.fix_type, -// gps.lat, -// gps.lon, -// gps.alt, -// cm_uint16_from_m_float(gps.eph), -// cm_uint16_from_m_float(gps.epv), -// gps.vel_m_s * 100.0f, -// _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, -// gps.satellites_used); -// } -// } -//}; -// -// +class MavlinkStreamGPSRawInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSRawInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_RAW_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + +protected: + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(nullptr), + _gps_time(0) + {} + + void subscribe() + { + _gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_gps_position_s gps; + + if (_gps_sub->update(&_gps_time, &gps)) { + mavlink_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); + } + } +}; + + //class MavlinkStreamGlobalPositionInt : public MavlinkStream //{ //public: -- cgit v1.2.3 From f1b55e578ff17d59cba13193cca4c83e764854b6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 11:02:56 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 713 ++++++++++++++++--------------- 1 file changed, 362 insertions(+), 351 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8f0c15f5a..fe33c1bea 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -877,15 +877,10 @@ private: protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _gps_sub(nullptr), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), _gps_time(0) {} - void subscribe() - { - _gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; @@ -910,339 +905,355 @@ protected: }; -//class MavlinkStreamGlobalPositionInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGlobalPositionInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GLOBAL_POSITION_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGlobalPositionInt(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// MavlinkOrbSubscription *home_sub; -// uint64_t home_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); -// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); -// -//protected: -// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0), -// home_sub(nullptr), -// home_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_global_position_s pos; -// struct home_position_s home; -// -// bool updated = pos_sub->update(&pos_time, &pos); -// updated |= home_sub->update(&home_time, &home); -// -// if (updated) { -// mavlink_msg_global_position_int_send(_channel, -// pos.timestamp / 1000, -// pos.lat * 1e7, -// pos.lon * 1e7, -// pos.alt * 1000.0f, -// (pos.alt - home.alt) * 1000.0f, -// pos.vel_n * 100.0f, -// pos.vel_e * 100.0f, -// pos.vel_d * 100.0f, -// _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -// } -// } -//}; -// -// -//class MavlinkStreamLocalPositionNED : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamLocalPositionNED::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "LOCAL_POSITION_NED"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_LOCAL_POSITION_NED; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamLocalPositionNED(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); -// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); -// -//protected: -// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_local_position_s pos; -// -// if (pos_sub->update(&pos_time, &pos)) { -// mavlink_msg_local_position_ned_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.vx, -// pos.vy, -// pos.vz); -// } -// } -//}; -// -// -// -//class MavlinkStreamViconPositionEstimate : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamViconPositionEstimate::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "VICON_POSITION_ESTIMATE"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamViconPositionEstimate(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); -// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); -// -//protected: -// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_vicon_position_s pos; -// -// if (pos_sub->update(&pos_time, &pos)) { -// mavlink_msg_vicon_position_estimate_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.roll, -// pos.pitch, -// pos.yaw); -// } -// } -//}; -// -// -//class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGPSGlobalOrigin::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GPS_GLOBAL_ORIGIN"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGPSGlobalOrigin(); -// } -// -//private: -// MavlinkOrbSubscription *home_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); -// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); -// -//protected: -// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), -// home_sub(nullptr) -// {} -// -// void subscribe() -// { -// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); -// } -// -// void send(const hrt_abstime t) -// { -// /* we're sending the GPS home periodically to ensure the -// * the GCS does pick it up at one point */ -// if (home_sub->is_published()) { -// struct home_position_s home; -// -// if (home_sub->update(&home)) { -// mavlink_msg_gps_global_origin_send(_channel, -// (int32_t)(home.lat * 1e7), -// (int32_t)(home.lon * 1e7), -// (int32_t)(home.alt) * 1000.0f); -// } -// } -// } -//}; -// -//template -//class MavlinkStreamServoOutputRaw : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamServoOutputRaw::get_name_static(); -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; -// } -// -// static const char *get_name_static() -// { -// switch (N) { -// case 0: -// return "SERVO_OUTPUT_RAW_0"; -// -// case 1: -// return "SERVO_OUTPUT_RAW_1"; -// -// case 2: -// return "SERVO_OUTPUT_RAW_2"; -// -// case 3: -// return "SERVO_OUTPUT_RAW_3"; -// } -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamServoOutputRaw(); -// } -// -//private: -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); -// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); -// -//protected: -// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), -// act_sub(nullptr), -// act_time(0) -// {} -// -// void subscribe() -// { -// orb_id_t act_topics[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; -// -// act_sub = _mavlink->add_orb_subscription(act_topics[N]); -// } -// -// void send(const hrt_abstime t) -// { -// struct actuator_outputs_s act; -// -// if (act_sub->update(&act_time, &act)) { -// mavlink_msg_servo_output_raw_send(_channel, -// act.timestamp / 1000, -// N, -// act.output[0], -// act.output[1], -// act.output[2], -// act.output[3], -// act.output[4], -// act.output[5], -// act.output[6], -// act.output[7]); -// } -// } -//}; -// -// +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + +protected: + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_global_position_s pos; + struct home_position_s home; + + bool updated = _pos_sub->update(&_pos_time, &pos); + updated |= _home_sub->update(&_home_time, &home); + + if (updated) { + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); + } + } +}; + + +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + +protected: + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_local_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); + } + } +}; + + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamViconPositionEstimate::get_name_static(); + } + + static const char *get_name_static() + { + return "VICON_POSITION_ESTIMATE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + +protected: + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_vicon_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); + } + } +}; + + +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSGlobalOrigin::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_GLOBAL_ORIGIN"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() + { + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_home_sub; + + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + +protected: + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) + {} + + void send(const hrt_abstime t) + { + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (_home_sub->is_published()) { + struct home_position_s home; + + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); + } + } + } +}; + + +template +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamServoOutputRaw::get_name_static(); + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + + static const char *get_name_static() + { + switch (N) { + case 0: + return "SERVO_OUTPUT_RAW_0"; + + case 1: + return "SERVO_OUTPUT_RAW_1"; + + case 2: + return "SERVO_OUTPUT_RAW_2"; + + case 3: + return "SERVO_OUTPUT_RAW_3"; + } + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamServoOutputRaw(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; + + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + +protected: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + } + + void send(const hrt_abstime t) + { + struct actuator_outputs_s act; + + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); + } + } +}; + + //class MavlinkStreamHILControls : public MavlinkStream //{ //public: @@ -1281,7 +1292,7 @@ protected: // MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); // //protected: -// explicit MavlinkStreamHILControls() : MavlinkStream(), +// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), // status_sub(nullptr), // status_time(0), // pos_sp_triplet_sub(nullptr), @@ -1414,7 +1425,7 @@ protected: // MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); // //protected: -// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), // pos_sp_triplet_sub(nullptr) // {} // @@ -1471,7 +1482,7 @@ protected: // MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); // //protected: -// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), // pos_sp_sub(nullptr), // pos_sp_time(0) // {} @@ -1529,7 +1540,7 @@ protected: // MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(mavlink), // att_sp_sub(nullptr), // att_sp_time(0) // {} @@ -1587,7 +1598,7 @@ protected: // MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(mavlink), // att_rates_sp_sub(nullptr), // att_rates_sp_time(0) // {} @@ -1645,7 +1656,7 @@ protected: // MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); // //protected: -// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(mavlink), // rc_sub(nullptr), // rc_time(0) // {} @@ -1739,7 +1750,7 @@ protected: // MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); // //protected: -// explicit MavlinkStreamManualControl() : MavlinkStream(), +// explicit MavlinkStreamManualControl() : MavlinkStream(mavlink), // manual_sub(nullptr), // manual_time(0) // {} @@ -1798,7 +1809,7 @@ protected: // MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); // //protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), // flow_sub(nullptr), // flow_time(0) // {} @@ -1856,7 +1867,7 @@ protected: // MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); // //protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), // att_ctrl_sub(nullptr), // att_ctrl_time(0) // {} @@ -1924,7 +1935,7 @@ protected: // MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); // //protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), // debug_sub(nullptr), // debug_time(0) // {} @@ -1981,7 +1992,7 @@ protected: // MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); // //protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), // status_sub(nullptr) // {} // @@ -2040,7 +2051,7 @@ protected: // MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); // //protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), // range_sub(nullptr), // range_time(0) // {} -- cgit v1.2.3 From e087ec81c396d7da8d5f7a006748062ee07b693f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 13:53:45 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 522 ++++++++++++++++--------------- 1 file changed, 268 insertions(+), 254 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33c1bea..14195b6a7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1254,260 +1254,274 @@ protected: }; -//class MavlinkStreamHILControls : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamHILControls::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "HIL_CONTROLS"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_HIL_CONTROLS; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamHILControls(); -// } -// -//private: -// MavlinkOrbSubscription *status_sub; -// uint64_t status_time; -// -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// uint64_t pos_sp_triplet_time; -// -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamHILControls(MavlinkStreamHILControls &); -// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); -// -//protected: -// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), -// status_sub(nullptr), -// status_time(0), -// pos_sp_triplet_sub(nullptr), -// pos_sp_triplet_time(0), -// act_sub(nullptr), -// act_time(0) -// {} -// -// void subscribe() -// { -// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_status_s status; -// struct position_setpoint_triplet_s pos_sp_triplet; -// struct actuator_outputs_s act; -// -// bool updated = act_sub->update(&act_time, &act); -// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); -// updated |= status_sub->update(&status_time, &status); -// -// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state; -// uint8_t mavlink_base_mode; -// uint32_t mavlink_custom_mode; -// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); -// -// float out[8]; -// -// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; -// -// /* scale outputs depending on system type */ -// if (mavlink_system.type == MAV_TYPE_QUADROTOR || -// mavlink_system.type == MAV_TYPE_HEXAROTOR || -// mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// /* multirotors: set number of rotor outputs depending on type */ -// -// unsigned n; -// -// switch (mavlink_system.type) { -// case MAV_TYPE_QUADROTOR: -// n = 4; -// break; -// -// case MAV_TYPE_HEXAROTOR: -// n = 6; -// break; -// -// default: -// n = 8; -// break; -// } -// -// for (unsigned i = 0; i < 8; i++) { -// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { -// if (i < n) { -// /* scale PWM out 900..2100 us to 0..1 for rotors */ -// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// -// } else { -// /* scale PWM out 900..2100 us to -1..1 for other channels */ -// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); -// } -// -// } else { -// /* send 0 when disarmed */ -// out[i] = 0.0f; -// } -// } -// -// } else { -// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ -// -// for (unsigned i = 0; i < 8; i++) { -// if (i != 3) { -// /* scale PWM out 900..2100 us to -1..1 for normal channels */ -// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); -// -// } else { -// /* scale PWM out 900..2100 us to 0..1 for throttle */ -// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// } -// -// } -// } -// -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } -// } -//}; -// -// -//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GLOBAL_POSITION_SETPOINT_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGlobalPositionSetpointInt(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); -// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); -// -//protected: -// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), -// pos_sp_triplet_sub(nullptr) -// {} -// -// void subscribe() -// { -// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// } -// -// void send(const hrt_abstime t) -// { -// struct position_setpoint_triplet_s pos_sp_triplet; -// -// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { -// mavlink_msg_global_position_setpoint_int_send(_channel, -// MAV_FRAME_GLOBAL, -// (int32_t)(pos_sp_triplet.current.lat * 1e7), -// (int32_t)(pos_sp_triplet.current.lon * 1e7), -// (int32_t)(pos_sp_triplet.current.alt * 1000), -// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); -// } -// } -//}; -// -// -//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamLocalPositionSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "LOCAL_POSITION_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamLocalPositionSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sp_sub; -// uint64_t pos_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); -// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); -// -//protected: -// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), -// pos_sp_sub(nullptr), -// pos_sp_time(0) -// {} -// -// void subscribe() -// { -// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_local_position_setpoint_s pos_sp; -// -// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { -// mavlink_msg_local_position_setpoint_send(_channel, -// MAV_FRAME_LOCAL_NED, -// pos_sp.x, -// pos_sp.y, -// pos_sp.z, -// pos_sp.yaw); -// } -// } -//}; -// -// +class MavlinkStreamHILControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHILControls::get_name_static(); + } + + static const char *get_name_static() + { + return "HIL_CONTROLS"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; + + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; + + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; + + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + +protected: + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + struct actuator_outputs_s act; + + bool updated = _act_sub->update(&_act_time, &act); + updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); + updated |= _status_sub->update(&_status_time, &status); + + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* multirotors: set number of rotor outputs depending on type */ + + unsigned n; + + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + for (unsigned i = 0; i < 8; i++) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } + + } else { + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } + } + + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); + } + } +}; + + +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + +protected: + explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + {} + + void send(const hrt_abstime t) + { + struct position_setpoint_triplet_s pos_sp_triplet; + + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_global_position_setpoint_int_t msg; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.latitude = pos_sp_triplet.current.lat * 1e7; + msg.longitude = pos_sp_triplet.current.lon * 1e7; + msg.altitude = pos_sp_triplet.current.alt * 1000; + msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + } + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sp_sub; + uint64_t _pos_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + +protected: + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_local_position_setpoint_s pos_sp; + + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_local_position_setpoint_t msg; + + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + } + } +}; + + //class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream //{ //public: -- cgit v1.2.3 From 67db8ee4f0908c31f17ed490c48ea21cc7ac3f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:27:10 +0200 Subject: Add missing states, build fix for master --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 ++++ src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++++ 2 files changed, 8 insertions(+) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3d504d395..ffdd29a5b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 6d1f47b68..a6b670c4d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -68,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; -- cgit v1.2.3 From 241802a71f2af56a8dd4510827f8ab5a59f5d6b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:40:53 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 584 ++++++++++++++++--------------- 1 file changed, 300 insertions(+), 284 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 14195b6a7..a710cdf6a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1522,275 +1522,291 @@ protected: }; -//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRollPitchYawThrustSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *att_sp_sub; -// uint64_t att_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); -// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); -// -//protected: -// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(mavlink), -// att_sp_sub(nullptr), -// att_sp_time(0) -// {} -// -// void subscribe() -// { -// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_setpoint_s att_sp; -// -// if (att_sp_sub->update(&att_sp_time, &att_sp)) { -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -// } -// } -//}; -// -// -//class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *att_rates_sp_sub; -// uint64_t att_rates_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); -// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); -// -//protected: -// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(mavlink), -// att_rates_sp_sub(nullptr), -// att_rates_sp_time(0) -// {} -// -// void subscribe() -// { -// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_rates_setpoint_s att_rates_sp; -// -// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, -// att_rates_sp.timestamp / 1000, -// att_rates_sp.roll, -// att_rates_sp.pitch, -// att_rates_sp.yaw, -// att_rates_sp.thrust); -// } -// } -//}; -// -// -//class MavlinkStreamRCChannelsRaw : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRCChannelsRaw::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "RC_CHANNELS_RAW"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_RC_CHANNELS_RAW; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRCChannelsRaw(); -// } -// -//private: -// MavlinkOrbSubscription *rc_sub; -// uint64_t rc_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); -// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); -// -//protected: -// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(mavlink), -// rc_sub(nullptr), -// rc_time(0) -// {} -// -// void subscribe() -// { -// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc)); -// } -// -// void send(const hrt_abstime t) -// { -// struct rc_input_values rc; -// -// if (rc_sub->update(&rc_time, &rc)) { -// const unsigned port_width = 8; -// -// // Deprecated message (but still needed for compatibility!) -// for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(_channel, -// rc.timestamp_publication / 1000, -// i, -// (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, -// rc.rssi); -// } -// -// // New message -// mavlink_msg_rc_channels_send(_channel, -// rc.timestamp_publication / 1000, -// rc.channel_count, -// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), -// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), -// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), -// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), -// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), -// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), -// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), -// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), -// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), -// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), -// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), -// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), -// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), -// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), -// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), -// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), -// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), -// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), -// rc.rssi); -// } -// } -//}; -// -// -//class MavlinkStreamManualControl : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamManualControl::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "MANUAL_CONTROL"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_MANUAL_CONTROL; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamManualControl(); -// } -// -//private: -// MavlinkOrbSubscription *manual_sub; -// uint64_t manual_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamManualControl(MavlinkStreamManualControl &); -// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); -// -//protected: -// explicit MavlinkStreamManualControl() : MavlinkStream(mavlink), -// manual_sub(nullptr), -// manual_time(0) -// {} -// -// void subscribe() -// { -// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct manual_control_setpoint_s manual; -// -// if (manual_sub->update(&manual_time, &manual)) { -// mavlink_msg_manual_control_send(_channel, -// mavlink_system.sysid, -// manual.x * 1000, -// manual.y * 1000, -// manual.z * 1000, -// manual.r * 1000, -// 0); -// } -// } -//}; -// -// +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sp_sub; + uint64_t att_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + +protected: + explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + att_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_attitude_setpoint_s att_sp; + + if (att_sp_sub->update(&att_sp_time, &att_sp)) { + mavlink_roll_pitch_yaw_thrust_setpoint_t msg; + + msg.time_boot_ms = att_sp.timestamp / 1000; + msg.roll = att_sp.roll_body; + msg.pitch = att_sp.pitch_body; + msg.yaw = att_sp.yaw_body; + msg.thrust = att_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); + } + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_rates_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); + MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + +protected: + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_rates_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_rates_setpoint_s att_rates_sp; + + if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { + mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; + + msg.time_boot_ms = att_rates_sp.timestamp / 1000; + msg.roll_rate = att_rates_sp.roll; + msg.pitch_rate = att_rates_sp.pitch; + msg.yaw_rate = att_rates_sp.yaw; + msg.thrust = att_rates_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + } + } +}; + + +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRCChannelsRaw::get_name_static(); + } + + static const char *get_name_static() + { + return "RC_CHANNELS_RAW"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() + { + return (RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; + + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + +protected: + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) + {} + + void send(const hrt_abstime t) + { + struct rc_input_values rc; + + if (_rc_sub->update(&_rc_time, &rc)) { + const unsigned port_width = 8; + + /* deprecated message (but still needed for compatibility!) */ + for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); + } + + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + } + } +}; + + +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamManualControl::get_name_static(); + } + + static const char *get_name_static() + { + return "MANUAL_CONTROL"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; + + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + +protected: + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) + {} + + void send(const hrt_abstime t) + { + struct manual_control_setpoint_s manual; + + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); + } + } +}; + + //class MavlinkStreamOpticalFlow : public MavlinkStream //{ //public: @@ -2109,21 +2125,21 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), -// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), -// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), -// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), -// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), -// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), -// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), -// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), -// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), + new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), + new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), + new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), // new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), // new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), // new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), -- cgit v1.2.3 From 019dc1b5264072c785433c53ca11995ed291f924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:46:35 +0200 Subject: mavlink: log message severity fixed --- src/modules/mavlink/mavlink_main.cpp | 27 ++++++++++++++++++++++----- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 19 +------------------ 3 files changed, 24 insertions(+), 24 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 408799be4..f9c7fcedf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -433,7 +433,24 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) const char *txt = (const char *)arg; struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - msg.severity = (unsigned char)cmd; + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -816,23 +833,23 @@ Mavlink::handle_message(const mavlink_message_t *msg) void Mavlink::send_statustext_info(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAV_SEVERITY_INFO, string); } void Mavlink::send_statustext_critical(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAV_SEVERITY_CRITICAL, string); } void Mavlink::send_statustext_emergency(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAV_SEVERITY_EMERGENCY, string); } void -Mavlink::send_statustext(unsigned severity, const char *string) +Mavlink::send_statustext(unsigned char severity, const char *string) { struct mavlink_logmessage logmsg; strncpy(logmsg.text, string, sizeof(logmsg.text)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4121e286e..38149cf10 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -218,7 +218,7 @@ public: * @param string the message to send (will be capped by mavlink max string length) * @param severity the log level */ - void send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned char severity, const char *string); MavlinkStream * get_streams() const { return _streams; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a710cdf6a..083a6301a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -351,24 +351,7 @@ protected: if (lb_ret == OK) { mavlink_statustext_t msg; - /* map severity */ - switch (logmsg.severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - msg.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - msg.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - msg.severity = MAV_SEVERITY_EMERGENCY; - break; - - default: - msg.severity = MAV_SEVERITY_INFO; - break; - } + msg.severity = logmsg.severity; strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); -- cgit v1.2.3 From f3ec46369b68d57489a5fa57a320ae99a1bda220 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 20:30:58 +0200 Subject: mavlink: all streams ported to new API --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 663 ++++++++++++++++--------------- 2 files changed, 347 insertions(+), 317 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f9c7fcedf..d72fb8508 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1350,7 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 083a6301a..683f0f1e8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1652,8 +1652,8 @@ public: unsigned get_size() { - return (RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1753,7 +1753,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1790,313 +1790,344 @@ protected: }; -//class MavlinkStreamOpticalFlow : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamOpticalFlow::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "OPTICAL_FLOW"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_OPTICAL_FLOW; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamOpticalFlow(); -// } -// -//private: -// MavlinkOrbSubscription *flow_sub; -// uint64_t flow_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); -// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); -// -//protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), -// flow_sub(nullptr), -// flow_time(0) -// {} -// -// void subscribe() -// { -// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); -// } -// -// void send(const hrt_abstime t) -// { -// struct optical_flow_s flow; -// -// if (flow_sub->update(&flow_time, &flow)) { -// mavlink_msg_optical_flow_send(_channel, -// flow.timestamp, -// flow.sensor_id, -// flow.flow_raw_x, flow.flow_raw_y, -// flow.flow_comp_x_m, flow.flow_comp_y_m, -// flow.quality, -// flow.ground_distance_m); -// } -// } -//}; -// -//class MavlinkStreamAttitudeControls : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitudeControls::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE_CONTROLS"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitudeControls(); -// } -// -//private: -// MavlinkOrbSubscription *att_ctrl_sub; -// uint64_t att_ctrl_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); -// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); -// -//protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), -// att_ctrl_sub(nullptr), -// att_ctrl_time(0) -// {} -// -// void subscribe() -// { -// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); -// } -// -// void send(const hrt_abstime t) -// { -// struct actuator_controls_s att_ctrl; -// -// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "rll ctrl ", -// att_ctrl.control[0]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "ptch ctrl ", -// att_ctrl.control[1]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "yaw ctrl ", -// att_ctrl.control[2]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "thr ctrl ", -// att_ctrl.control[3]); -// } -// } -//}; -// -//class MavlinkStreamNamedValueFloat : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamNamedValueFloat::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "NAMED_VALUE_FLOAT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamNamedValueFloat(); -// } -// -//private: -// MavlinkOrbSubscription *debug_sub; -// uint64_t debug_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); -// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); -// -//protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), -// debug_sub(nullptr), -// debug_time(0) -// {} -// -// void subscribe() -// { -// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); -// } -// -// void send(const hrt_abstime t) -// { -// struct debug_key_value_s debug; -// -// if (debug_sub->update(&debug_time, &debug)) { -// /* enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(_channel, -// debug.timestamp_ms, -// debug.key, -// debug.value); -// } -// } -//}; -// -//class MavlinkStreamCameraCapture : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamCameraCapture::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "CAMERA_CAPTURE"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamCameraCapture(); -// } -// -//private: -// MavlinkOrbSubscription *status_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); -// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); -// -//protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), -// status_sub(nullptr) -// {} -// -// void subscribe() -// { -// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_status_s status; -// (void)status_sub->update(&status); -// -// if (status.arming_state == ARMING_STATE_ARMED -// || status.arming_state == ARMING_STATE_ARMED_ERROR) { -// -// /* send camera capture on */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -// -// } else { -// /* send camera capture off */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -// } -// } -//}; -// -//class MavlinkStreamDistanceSensor : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamDistanceSensor::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "DISTANCE_SENSOR"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_DISTANCE_SENSOR; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamDistanceSensor(); -// } -// -//private: -// MavlinkOrbSubscription *range_sub; -// uint64_t range_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); -// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); -// -//protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), -// range_sub(nullptr), -// range_time(0) -// {} -// -// void subscribe() -// { -// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); -// } -// -// void send(const hrt_abstime t) -// { -// struct range_finder_report range; -// -// if (range_sub->update(&range_time, &range)) { -// -// uint8_t type; -// -// switch (range.type) { -// case RANGE_FINDER_TYPE_LASER: -// type = MAV_DISTANCE_SENSOR_LASER; -// break; -// } -// -// uint8_t id = 0; -// uint8_t orientation = 0; -// uint8_t covariance = 20; -// -// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, -// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); -// } -// } -//}; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamOpticalFlow::get_name_static(); + } + + static const char *get_name_static() + { + return "OPTICAL_FLOW"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() + { + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; + + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + +protected: + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) + {} + + void send(const hrt_abstime t) + { + struct optical_flow_s flow; + + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + } + } +}; + +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeControls::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_CONTROLS"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() + { + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + +protected: + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) + {} + + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; + + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNamedValueFloat::get_name_static(); + } + + static const char *get_name_static() + { + return "NAMED_VALUE_FLOAT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; + + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + +protected: + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) + {} + + void send(const hrt_abstime t) + { + struct debug_key_value_s debug; + + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); + /* enforce null termination */ + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraCapture::get_name_static(); + } + + static const char *get_name_static() + { + return "CAMERA_CAPTURE"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + +protected: + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + {} + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + (void)_status_sub->update(&status); + + mavlink_command_long_t msg; + + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } +}; + + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDistanceSensor::get_name_static(); + } + + static const char *get_name_static() + { + return "DISTANCE_SENSOR"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() + { + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; + + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + +protected: + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) + {} + + void send(const hrt_abstime t) + { + struct range_finder_report range; + + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; + + msg.time_boot_ms = range.timestamp / 1000; + + switch (range.type) { + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; + + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); + } + } +}; StreamListItem *streams_list[] = { @@ -2111,6 +2142,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), @@ -2123,11 +2155,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), -// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), -// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), -// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), -// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), -// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), -// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), nullptr }; -- cgit v1.2.3 From 129c93f22f0717d9d85910de87fecece9fabbc79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:11:58 +0200 Subject: Revert "Remove all unused TECS parameters" This reverts commit ce78b399691d43bd150c0a5928f08c98076a3892. Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 57 +++++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 176 +++++++++++++++++++++ 2 files changed, 233 insertions(+) (limited to 'src/modules') 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 0b111f7bd..a1bc5c24e 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 @@ -199,6 +199,19 @@ private: float l1_period; float l1_damping; + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + float airspeed_min; float airspeed_trim; float airspeed_max; @@ -226,6 +239,19 @@ private: param_t l1_period; param_t l1_damping; + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -438,6 +464,21 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); + /* fetch initial parameter values */ parameters_update(); } @@ -488,6 +529,22 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); 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 27039ff51..52128e1b7 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 @@ -154,6 +154,182 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +/** + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + +/** + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second square) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + +/** + * Height rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); + /** * Landing slope angle * -- cgit v1.2.3 From ae2de675014c701b45710b646f3a816ad2222b73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:24:11 +0200 Subject: Revert "Remove old TECS implementation - we can really only decently flight-test and support one." This reverts commit 503ded05394767d58359834e73bc63672b701dbe. Conflicts: mavlink/include/mavlink/v1.0 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- makefiles/config_aerocore_default.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 74 +++++++++++++++++----- 4 files changed, 61 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 53a2ad1ab..0d3934bd0 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,6 +80,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29..a7c10f52f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -108,6 +108,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b40..d0a40445d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -121,6 +121,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion 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 a1bc5c24e..78e91bbfd 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 @@ -42,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -87,6 +87,7 @@ #include #include #include +#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -192,6 +193,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -557,6 +559,23 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -618,6 +637,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -790,6 +812,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + /* filter speed and altitude for controller */ + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; + + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -815,6 +845,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1123,9 +1156,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1305,20 +1338,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int -- cgit v1.2.3 From 5171286bbb7eef115bd7fc711f2c8fcebd432e4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:34:46 +0200 Subject: Re-add two params that fell off the truck before --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules') 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 78e91bbfd..f0e18d1bb 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 @@ -205,6 +205,8 @@ private: float min_sink_rate; float max_sink_rate; float max_climb_rate; + float heightrate_p; + float speedrate_p; float throttle_damp; float integrator_gain; float vertical_accel_limit; @@ -245,6 +247,8 @@ private: param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t heightrate_p; + param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; -- cgit v1.2.3 From 2cf688312a83d3c8ad5b813de0643901891886c0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 11:58:59 +0200 Subject: mavlink: show 'rate mult' in 'mavlink status' output --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4ae90c344..69beb9cc9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1660,6 +1660,7 @@ Mavlink::display_status() printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int -- cgit v1.2.3 From 7f2799a78baf022db05484df2168b1b5a89923a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:49:51 +0200 Subject: global pos topic docs --- src/modules/uORB/topics/vehicle_global_position.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e32529cb4..25137c1c6 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -70,8 +70,8 @@ struct vehicle_global_position_s { float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; - float epv; + float eph; /**< Standard deviation of position estimate horizontally */ + float epv; /**< Standard deviation of position vertically */ }; /** -- cgit v1.2.3 From 0cbd943d093f2cb69102e57ef7964b5dbebd9f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:50:52 +0200 Subject: Differentiate between EPH and EPV. Do not declare position invalid because of EPV (because we use the baro anyway). No fundamental logic change / cleanup to ensure current approach and arming logic remains intact --- src/modules/commander/commander.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c24734e5..dec5ab46c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -163,7 +163,8 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; -static float eph_epv_threshold = 5.0f; +static float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -1108,32 +1109,32 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1163,8 +1164,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1172,7 +1173,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { @@ -1504,7 +1505,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; -- cgit v1.2.3 From e3da7f564f1bc7746ba3be946fd02b95642cbf6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 21:36:55 +0200 Subject: NaN check and better init in lowpass --- src/modules/controllib/blocks.cpp | 3 +++ src/modules/controllib/blocks.hpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 0175acda9..f739446fa 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 37d7832b3..bffc355a8 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; -- cgit v1.2.3 From 1dc23d0c49d99fa93284a277a6bc4970ac0e7b3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:41:45 +0200 Subject: Disable mTECS until runtime error is better understood --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 56 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + 2 files changed, 29 insertions(+), 28 deletions(-) (limited to 'src/modules') 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 f0e18d1bb..2d6c5c121 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 @@ -194,7 +194,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - fwPosctrl::mTecs _mTecs; + // fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - _mTecs(), + // _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - _mTecs.updateParams(); + // _mTecs.updateParams(); return OK; } @@ -820,9 +820,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + // if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } + // } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - } + // if (_mTecs.getEnabled()) { + // _mTecs.resetIntegrators(); + // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + // } } _was_pos_control_mode = true; @@ -1160,9 +1160,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1342,29 +1342,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); - } else { + // if (_mTecs.getEnabled()) { + // /* Using mtecs library: prepare arguments for mtecs call */ + // float flightPathAngle = 0.0f; + // float ground_speed_length = ground_speed.length(); + // if (ground_speed_length > FLT_EPSILON) { + // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + // } + // fwPosctrl::LimitOverride limitOverride; + // if (climbout_mode) { + // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + // } else { + // limitOverride.disablePitchMinOverride(); + // } + // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + // limitOverride); + // } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - } + // } } int diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..afc411a60 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,6 +76,7 @@ mTecs::mTecs() : _counter(0), _debug(false) { + warnx("starting"); } mTecs::~mTecs() -- cgit v1.2.3 From 39aa9112baf0120538255cf44292ff8e1ff3387c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:56:17 +0200 Subject: Fix threading in FW pos controller --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 87 +++++++++++----------- 1 file changed, 43 insertions(+), 44 deletions(-) (limited to 'src/modules') 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 2d6c5c121..eadb63f40 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 @@ -92,6 +92,8 @@ #include "landingslope.h" #include "mtecs/mTecs.h" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -116,9 +118,9 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ - int start(); + static int start(); /** * Task status @@ -132,7 +134,6 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ int _global_pos_sub; int _pos_sp_triplet_sub; @@ -194,7 +195,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - // fwPosctrl::mTecs _mTecs; + fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -393,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), _task_running(false), - _control_task(-1), /* subscriptions */ _global_pos_sub(-1), @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - // _mTecs(), + _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - // _mTecs.updateParams(); + _mTecs.updateParams(); return OK; } @@ -704,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -820,9 +830,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - // if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - // } + } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +848,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - // if (_mTecs.getEnabled()) { - // _mTecs.resetIntegrators(); - // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - // } + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } } _was_pos_control_mode = true; @@ -1160,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1178,10 +1188,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1342,29 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - // if (_mTecs.getEnabled()) { - // /* Using mtecs library: prepare arguments for mtecs call */ - // float flightPathAngle = 0.0f; - // float ground_speed_length = ground_speed.length(); - // if (ground_speed_length > FLT_EPSILON) { - // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - // } - // fwPosctrl::LimitOverride limitOverride; - // if (climbout_mode) { - // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - // } else { - // limitOverride.disablePitchMinOverride(); - // } - // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - // limitOverride); - // } else { + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); + } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - // } + } } int @@ -1398,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (l1_control::g_control != nullptr) errx(1, "already running"); - l1_control::g_control = new FixedwingPositionControl; - - if (l1_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != l1_control::g_control->start()) { - delete l1_control::g_control; - l1_control::g_control = nullptr; + if (OK != FixedwingPositionControl::start()) { err(1, "start failed"); } -- cgit v1.2.3 From 05e253b357caca3cdeb274027c7627a8a59d190d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:49:01 +0200 Subject: Proper locking for message send calls --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 17 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 49781e8f8..8c79a1fee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,9 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, + _rstatus{}, + _message_buffer{}, + _message_buffer_mutex{}, + _send_mutex{}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -716,7 +717,7 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: @@ -742,6 +743,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } + pthread_mutex_lock(_send_mutex); + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; @@ -754,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - return; + goto out; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -762,7 +765,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; - buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; buf[4] = mavlink_system.compid; buf[5] = msgid; @@ -790,6 +794,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + +out: + pthread_mutex_unlock(_send_mutex); } void @@ -1298,6 +1305,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 38149cf10..1e2e94cef 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -342,6 +342,7 @@ private: mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; bool _param_initialized; param_t _param_system_id; -- cgit v1.2.3 From 5bad95c92dc7b974a793074ca173fe129d258fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:52:02 +0200 Subject: make GCC happy --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8c79a1fee..618b9fa29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -743,7 +743,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } - pthread_mutex_lock(_send_mutex); + pthread_mutex_lock(&_send_mutex); int buf_free = get_free_tx_buf(); @@ -757,7 +757,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - goto out; + pthread_mutex_unlock(&_send_mutex); + return; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -795,8 +796,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) count_txbytes(packet_len); } -out: - pthread_mutex_unlock(_send_mutex); + pthread_mutex_unlock(&_send_mutex); } void -- cgit v1.2.3 From 391a5c82cbe6d02136e16528b472352e8461b02a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:54:40 +0200 Subject: Print avionics voltage --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daba4e740..409cf9d89 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -335,6 +335,7 @@ void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); -- cgit v1.2.3 From edccc826a1834aa21171e2b5f88c97454955bb16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 21:16:29 +0200 Subject: Improved comment of bridge header --- src/modules/mavlink/mavlink_bridge_header.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index d82c2dae7..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes -- cgit v1.2.3 From fbbfe61d745202f7163d874ab61d4568f3a18a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 10:49:28 +0200 Subject: Add missing lock --- src/modules/mavlink/mavlink_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 618b9fa29..84422c4aa 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -808,6 +808,8 @@ Mavlink::resend_message(mavlink_message_t *msg) return; } + pthread_mutex_lock(&_send_mutex); + int buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); @@ -819,6 +821,7 @@ Mavlink::resend_message(mavlink_message_t *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); return; } @@ -842,6 +845,8 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + + pthread_mutex_unlock(&_send_mutex); } void -- cgit v1.2.3 From 2b71067618b2b4333c44072e27b960435407166d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 11:28:17 +0200 Subject: Code style fixes --- src/modules/mavlink/mavlink_main.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 84422c4aa..5589738c1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,10 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _send_mutex{}, + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -387,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) /* if not in normal mode, we are an onboard link * onboard links should only pass on messages from the same system ID */ - if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { inst->pass_message(msg); } } @@ -731,6 +731,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } + return buf_free; } @@ -778,7 +779,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); crc_accumulate(mavlink_message_crcs[msgid], &checksum); buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -985,7 +986,7 @@ Mavlink::adjust_stream_rates(const float multiplier) /* allow max ~600 Hz */ if (interval < 1600) { interval = 1600; - } + } /* set new interval */ stream->set_interval(interval * multiplier); @@ -1513,6 +1514,7 @@ Mavlink::task_main(int argc, char *argv[]) _bytes_txerr = 0; _bytes_rx = 0; } + _bytes_timestamp = t; } @@ -1676,6 +1678,7 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); -- cgit v1.2.3 From 39699670026f463df79a45c0dbf5cf7ceea8d5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:16:11 +0200 Subject: Default to TECS --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 7cfe63fbc..58a1e9f6b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); +PARAM_DEFINE_INT32(MT_ENABLED, 0); /** * Total Energy Rate Control Feedforward -- cgit v1.2.3 From 7f293be7d77603768899aedb438821dd19b8b4d7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 31 Jul 2014 17:30:00 +0200 Subject: mavlink, rc.usb: increase HIL_CONTROLS rate and datarate on USB to allow HIL simulation @ 200Hz --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index eb6039b4c..f1c21f295 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 10000 -d /dev/ttyACM0 -x +mavlink start -r 20000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 usleep 100000 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5589738c1..9a39d3bed 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate #define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN @@ -694,7 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - configure_stream("HIL_CONTROLS", 150.0f); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ -- cgit v1.2.3 From 87273abc9aa0006baff53741e605c9992ec8c955 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 00:56:44 +0400 Subject: UAVCAN node version info --- src/modules/uavcan/uavcan_main.cpp | 61 +++++++++++++++++++++++++++++--------- src/modules/uavcan/uavcan_main.hpp | 1 + 2 files changed, 48 insertions(+), 14 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 27e77e9c5..ffd599070 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include @@ -173,6 +175,44 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return OK; } +void UavcanNode::fill_node_info() +{ + /* software version */ + uavcan::protocol::SoftwareVersion swver; + + // Extracting the last 8 hex digits of FW_GIT and converting them to int + const unsigned fw_git_len = std::strlen(FW_GIT); + if (fw_git_len >= 8) { + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + } + + warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + + _node.setSoftwareVersion(swver); + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + + _node.setHardwareVersion(hwver); +} + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -183,6 +223,13 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + _node.setName("org.pixhawk.pixhawk"); + + _node.setNodeID(node_id); + + fill_node_info(); + + /* initializing the bridges UAVCAN <--> uORB */ ret = _esc_controller.init(); if (ret < 0) return ret; @@ -191,20 +238,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - uavcan::protocol::SoftwareVersion swver; - swver.major = 12; // TODO fill version info - swver.minor = 34; - _node.setSoftwareVersion(swver); - - uavcan::protocol::HardwareVersion hwver; - hwver.major = 42; // TODO fill version info - hwver.minor = 42; - _node.setHardwareVersion(hwver); - - _node.setName("org.pixhawk"); // Huh? - - _node.setNodeID(node_id); - return _node.start(); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 443525379..05b66fd7b 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: static UavcanNode* instance() { return _instance; } private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); int run(); -- cgit v1.2.3 From b606674fc77aa772fba66ad09dd3879a62a7f44c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 01:04:24 +0400 Subject: Removed misleading comment from the UAVCAN module makefile --- src/modules/uavcan/module.mk | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1ef6f0cfa..3865f2468 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # # Invoke DSDL compiler -# TODO: Add make target for this, or invoke dsdlc manually. -# The second option assumes that the generated headers shall be saved -# under the version control, which may be undesirable. -# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated -- cgit v1.2.3 From 3b1c92e42f57277a12d40963c22fbea9820ee0ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:10:07 +0200 Subject: Make throttle time constant tunable separately, group TECS params correctly, make climbout alt configurable --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ++++- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 49 ++++++++++++++++------ 2 files changed, 46 insertions(+), 14 deletions(-) (limited to 'src/modules') 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 eadb63f40..f05f20cd0 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 @@ -203,9 +203,11 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; float heightrate_p; float speedrate_p; float throttle_damp; @@ -245,9 +247,11 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; param_t heightrate_p; param_t speedrate_p; param_t throttle_damp; @@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); @@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); @@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); @@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, 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 52128e1b7..a0d9309b9 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 @@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + /** * Maximum climb rate * @@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Smaller values make it faster to respond, larger values make it slower * to respond. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + /** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); @@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Speed rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -- cgit v1.2.3 From 2e4dff3fea2177538f3549cedd8f84fbbbecd4ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:21:30 +0200 Subject: removed debug statement from mTECS --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index afc411a60..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,7 +76,6 @@ mTecs::mTecs() : _counter(0), _debug(false) { - warnx("starting"); } mTecs::~mTecs() -- cgit v1.2.3 From 32e32d92bdad3eb4e92e9e53c112fcdcf848c7e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:39:24 +0200 Subject: Add throttle slew rate param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+) (limited to 'src/modules') 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 f05f20cd0..189a19d48 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 @@ -229,6 +229,7 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; @@ -273,6 +274,7 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; @@ -464,6 +466,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -538,6 +541,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); @@ -576,6 +580,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); _tecs.set_integrator_gain(_parameters.integrator_gain); _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); 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 a0d9309b9..41c374407 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 @@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + /** * Negative pitch limit * -- cgit v1.2.3 From c3666a5504fc89b7e1861e25c9b6cb0c07453784 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:18:43 +0200 Subject: Widen default first WP radius --- src/modules/navigator/mission_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 881caa24e..d15e1e771 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * waypoint is more distant than MIS_DIS_1WP from the current position. * * @min 0 - * @max 250 + * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); -- cgit v1.2.3 From 4d03202c4c8c322f9d93da0051dca015ab473cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:42:26 +0200 Subject: Warn on far waypoints --- src/modules/navigator/mission.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 12f6b9c21..c0e37a3ed 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -273,6 +273,10 @@ Mission::check_dist_1wp() if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } return true; } else { -- cgit v1.2.3 From 9725a1635254ba6c0c1df0413f94398ec28e23f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 2 Aug 2014 03:34:57 +0400 Subject: UAVCAN: Fixed short git hash computation --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ffd599070..9d5404baf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -180,16 +180,13 @@ void UavcanNode::fill_node_info() /* software version */ uavcan::protocol::SoftwareVersion swver; - // Extracting the last 8 hex digits of FW_GIT and converting them to int - const unsigned fw_git_len = std::strlen(FW_GIT); - if (fw_git_len >= 8) { - char fw_git_short[9] = {}; - std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); - assert(fw_git_short[8] == '\0'); - char *end = nullptr; - swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; - } + // Extracting the first 8 hex digits of FW_GIT and converting them to int + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); -- cgit v1.2.3 From 5824607c76ed38b5c08d1c9031814f741bce7c8b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Aug 2014 13:01:42 +0200 Subject: uavcan: fix actuator groups subcriptions and poll() --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9d5404baf..5b1539e3e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -253,7 +253,6 @@ int UavcanNode::run() // XXX figure out the output count _output_count = 2; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); actuator_outputs_s outputs; @@ -273,21 +272,23 @@ int UavcanNode::run() _node.setStatusOk(); - while (!_task_should_exit) { + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds_num = 0; + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -301,7 +302,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { @@ -317,7 +318,7 @@ int UavcanNode::run() // XXX trigger failsafe } - //can we mix? + // can we mix? if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -417,7 +418,8 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; + // the first fd used by CAN + _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -523,8 +525,8 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } /* -- cgit v1.2.3 From 020ac409be62e6c459e7aa5cb3181941c9bbfd23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:17:18 +0200 Subject: Remove airspeed and altitude raw data from TECS log, as we have these quantities already in the system log --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 ++---- src/modules/sdlog2/sdlog2.c | 6 ++---- src/modules/sdlog2/sdlog2_messages.h | 4 +--- src/modules/uORB/topics/tecs_status.h | 12 ++++++------ 4 files changed, 11 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..bffeefc1f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Write part of the status message */ _status.altitudeSp = altitudeSp; - _status.altitude = altitude; - _status.altitudeFiltered = altitudeFiltered; + _status.altitude_filtered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ @@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Write part of the status message */ _status.airspeedSp = airspeedSp; - _status.airspeed = airspeed; - _status.airspeedFiltered = airspeedFiltered; + _status.airspeed_filtered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 31861c3fc..6c4b49452 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; - log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 853a3811f..fb7609435 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -333,13 +333,11 @@ struct log_GS1B_s { #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; - float altitude; float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = { 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"), - LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 33055018c..ddca19d61 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -51,11 +51,13 @@ */ typedef enum { - TECS_MODE_NORMAL, + TECS_MODE_NORMAL = 0, TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF, TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM + TECS_MODE_LAND_THROTTLELIM, + TECS_MODE_BAD_DESCENT, + TECS_MODE_CLIMBOUT } tecs_mode; /** @@ -65,14 +67,12 @@ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ float altitudeSp; - float altitude; - float altitudeFiltered; + float altitude_filtered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; - float airspeedFiltered; + float airspeed_filtered; float airspeedDerivativeSp; float airspeedDerivative; -- cgit v1.2.3 From ff760d07b4d115ce0bc58ca8e8f98a915c4bbb67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:19:10 +0200 Subject: Add TECS logging --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 47 ++++++++++++++++++++++ 1 file changed, 47 insertions(+) (limited to 'src/modules') 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 189a19d48..350ce6dec 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 @@ -146,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), /* states */ @@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } } -- cgit v1.2.3 From 6fbeaeaacecfa532c1c4437ba51312e1c46d980d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:40:38 -0700 Subject: Update arming_state_transition calls Add new fRunPreArmChecks flag --- src/modules/commander/commander.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2b528eef6..4f976546e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } -- cgit v1.2.3 From 014fd5f47baad2f49292216eba851100aa543fef Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:41:29 -0700 Subject: Add fRunPreArmChecks flag This is to allow unit tests to be written which do not perform pre-arm checks --- src/modules/commander/state_machine_helper.cpp | 13 +++++++------ src/modules/commander/state_machine_helper.h | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3c3d2f233..f8589d24b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = { }; transition_result_t -arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); @@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current int prearm_ret = OK; /* only perform the check if we have to */ - if (new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..69ce8bbce 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -- cgit v1.2.3 From cfe5c45e04f65f24110f222c3de02147146081e1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:42:16 -0700 Subject: Fix broken arming unit test Also, update arming_state_transition calls to add new fRunPreArmChecks flag. --- .../commander/commander_tests/state_machine_helper_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2e18c4284..08dda2fab 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) MTT_ALL_NOT_VALID, MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, - { "transition: MANUAL to AUTO_MISSION - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, - { "transition: AUTO_MISSION to MANUAL - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", -- cgit v1.2.3 From 33762ce861b06cfa521d75fc18df3c7237e3a423 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 6 Aug 2014 12:40:07 +0400 Subject: UAVCAN ESC mixer: removed the failsafe placeholder, it's no use here --- src/modules/uavcan/uavcan_main.cpp | 5 ----- 1 file changed, 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b1539e3e..4535b6d6a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -313,11 +313,6 @@ int UavcanNode::run() } } - if (!controls_updated) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } - // can we mix? if (controls_updated && (_mixers != nullptr)) { -- cgit v1.2.3 From 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:25:20 +0200 Subject: Do not fuse height after innovation consistency check fail --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ffdd29a5b..c619735c6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED() // apply a 10-sigma threshold current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode) { current_ekf_state.hgtHealth = true; current_ekf_state.hgtFailTime = millis(); + + // if we just reset from a timeout, do not fuse + // the height data, but reset height and stored states + if (current_ekf_state.hgtTimeout) { + ResetHeight(); + ResetStoredStates(); + fuseHgtData = false; + } } else { -- cgit v1.2.3 From 20c3a329c72ca5950f7bb037473a812b4dd74b1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:36:59 +0200 Subject: Introduce similar checks fo all other health checks in the filter --- .../ekf_att_pos_estimator/estimator_23states.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c619735c6..c7c7305b2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { -- cgit v1.2.3 From faaeaeb11333598abd17db2189a2bc47216d0a98 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:22:57 +0200 Subject: mc_pos_control: manual and offboard control reorganization and cleanup --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 271 +++++++++++---------- .../uORB/topics/position_setpoint_triplet.h | 8 +- 2 files changed, 140 insertions(+), 139 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d22f43b5a..a0837a2dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -105,6 +105,7 @@ public: int start(); private: + const float alt_ctl_dz = 0.2f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ @@ -184,6 +185,8 @@ private: math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -216,6 +219,16 @@ private: */ void reset_alt_sp(); + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + /** * Select between barometric and global (AMSL) altitudes */ @@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode.flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual.x; + _sp_move_rate(1) = _manual.y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet.current.vx; + _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet.current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -553,13 +682,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - - math::Vector<3> sp_move_rate; - sp_move_rate.zero(); - - float yaw_sp_move_rate; - math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -618,138 +740,17 @@ MulticopterPositionControl::task_main() _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; - sp_move_rate.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); - } - - if (_control_mode.flag_control_position_enabled) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.x; - sp_move_rate(1) = _manual.y; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } + control_manual(dt); } else if (_control_mode.flag_control_offboard_enabled) { - /* Offboard control */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - - if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { - - _pos_sp(0) = _pos_sp_triplet.current.x; - _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _pos_sp_triplet.current.vx; - sp_move_rate(1) = _pos_sp_triplet.current.vy; - yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; - _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; - } - - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } - - } else { - reset_pos_sp(); - reset_alt_sp(); - } + /* offboard control */ + control_offboard(dt); } else { /* AUTO */ @@ -823,7 +824,7 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; @@ -903,7 +904,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a1932180..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,7 +60,7 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ }; struct position_setpoint_s @@ -71,9 +71,9 @@ struct position_setpoint_s float y; /**< local position setpoint in m in NED */ float z; /**< local position setpoint in m in NED */ bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m in NED */ - float vy; /**< local velocity setpoint in m in NED */ - float vz; /**< local velocity setpoint in m in NED */ + float vx; /**< local velocity setpoint in m/s in NED */ + float vy; /**< local velocity setpoint in m/s in NED */ + float vz; /**< local velocity setpoint in m/s in NED */ bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ -- cgit v1.2.3 From 03f839a27ac61178be8ef77526faa13a46c4cd6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:24:18 +0200 Subject: mc_pos_control: more accurate position setpoint reset, keep attitude setpoint continuous --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a0837a2dd..0106af80a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -514,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -525,7 +528,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 From 5225d87854bdea1b5e4367e3db6b41ece9e46e13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:24:04 +0200 Subject: Updated MAVLink to latest revision --- src/modules/mavlink/mavlink_messages.cpp | 151 ++++++++++--------------------- src/modules/mavlink/mavlink_receiver.cpp | 34 ------- 2 files changed, 49 insertions(+), 136 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 683f0f1e8..7f67475f2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1385,43 +1385,43 @@ protected: }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + return new MavlinkStreamPositionTargetGlobalInt(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} @@ -1430,15 +1430,14 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_global_position_setpoint_int_t msg; + mavlink_position_target_global_int_t msg; msg.coordinate_frame = MAV_FRAME_GLOBAL; - msg.latitude = pos_sp_triplet.current.lat * 1e7; - msg.longitude = pos_sp_triplet.current.lon * 1e7; - msg.altitude = pos_sp_triplet.current.alt * 1000; - msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; - _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1454,12 +1453,12 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } static MavlinkStream *new_instance(Mavlink *mavlink) @@ -1469,7 +1468,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -1491,137 +1490,86 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_local_position_setpoint_t msg; + mavlink_position_target_local_ned_t msg; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; - msg.yaw = pos_sp.yaw; - _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + return new MavlinkStreamAttitudeTarget(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_roll_pitch_yaw_thrust_setpoint_t msg; - - msg.time_boot_ms = att_sp.timestamp / 1000; - msg.roll = att_sp.roll_body; - msg.pitch = att_sp.pitch_body; - msg.yaw = att_sp.yaw_body; - msg.thrust = att_sp.thrust; - - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); - } - } -}; - + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } + mavlink_attitude_target_t msg; - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); - } - - unsigned get_size() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - MavlinkOrbSubscription *_att_rates_sp_sub; - uint64_t _att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), - _att_rates_sp_time(0) - {} - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; + msg.time_boot_ms = att_sp.timestamp / 1000; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); - if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; - msg.time_boot_ms = att_rates_sp.timestamp / 1000; - msg.roll_rate = att_rates_sp.roll; - msg.pitch_rate = att_rates_sp.pitch; - msg.yaw_rate = att_rates_sp.yaw; - msg.thrust = att_rates_sp.thrust; + msg.thrust = att_sp.thrust; - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -2149,10 +2097,9 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 69e3ef31d..63bac45c0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } } -void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) -{ - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } - } -} - void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { -- cgit v1.2.3 From 9ecec7fada7da3778c6214defcef68ea05352027 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:30:25 +0200 Subject: Add default initializers and timestamp in local position --- src/modules/mavlink/mavlink_messages.cpp | 7 ++++--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + src/modules/uORB/topics/vehicle_local_position_setpoint.h | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7f67475f2..c10be77b5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1430,7 +1430,7 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg; + mavlink_position_target_global_int_t msg{}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1490,8 +1490,9 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg; + mavlink_position_target_local_ned_t msg{}; + msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; @@ -1558,7 +1559,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg; + mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0106af80a..ecc40428d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -788,6 +788,7 @@ MulticopterPositionControl::task_main() } /* fill local position setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index 8988a0330..6766bb58a 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -50,11 +50,12 @@ */ struct vehicle_local_position_setpoint_s { + uint64_t timestamp; /**< timestamp of the setpoint */ float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame to go to */ +}; /**< Local position in NED frame */ /** * @} -- cgit v1.2.3 From 1d3edfa62976bdf73eeef67309c67b154be07810 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 10 Aug 2014 12:02:10 -0700 Subject: More FTP support Added: - sequence numbers - directory list returns file sizes - open command returns file size in data --- src/modules/mavlink/mavlink_ftp.cpp | 48 +++++++++++++++++++++++++++++++------ src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 42 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a2c900af..17d1babff 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -190,6 +191,8 @@ void MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); + + hdr->magic = kProtocolMagic; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; @@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; // store the type marker switch (entry.d_type) { case DTYPE_FILE: hdr->data[offset++] = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } break; case DTYPE_DIRECTORY: hdr->data[offset++] = kDirentDir; @@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req) hdr->data[offset++] = kDirentUnknown; break; } + + if (entry.d_type == DTYPE_FILE) { + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + // name too big to fit? + if ((nameLen + offset + 2) > kMaxDataLength) { + break; + } + // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); + strcpy((char *)&hdr->data[offset], buf); //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); - offset += strlen(entry.d_name) + 1; + offset += nameLen + 1; } closedir(dp); @@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create) return kErrNoSession; } + + uint32_t fileSize = 0; + if (!create) { + struct stat st; + if (stat(req->dataAsCString(), &st) != 0) { + return kErrNotFile; + } + fileSize = st.st_size; + } + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; int fd = ::open(req->dataAsCString(), oflag); @@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create) _session_fds[session_index] = fd; hdr->session = session_index; - hdr->size = 0; + if (create) { + hdr->size = 0; + } else { + hdr->size = sizeof(uint32_t); + *((uint32_t*)hdr->data) = fileSize; + } return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1bd1158fb..800c98b69 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -146,7 +146,7 @@ private: mavlink_message_t msg; msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); + _mavlink->get_channel(), &msg, sequence()+1, rawData()); _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); -- cgit v1.2.3