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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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 08055e5d52b8c522ca86e86bc161343dc396e5d9 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Mon, 17 Feb 2014 11:45:51 +0000 Subject: added examples/jetdrive_control(copy of mc_att_control) --- .gitignore | 6 + makefiles/config_px4fmu-v2_default.mk | 1 + src/examples/jetdrive_control/jetdrive_control.h | 207 +++++++ .../jetdrive_control/jetdrive_control_main.cpp | 678 +++++++++++++++++++++ .../jetdrive_control/jetdrive_control_params.c | 56 ++ src/examples/jetdrive_control/module.mk | 41 ++ 6 files changed, 989 insertions(+) create mode 100644 src/examples/jetdrive_control/jetdrive_control.h create mode 100644 src/examples/jetdrive_control/jetdrive_control_main.cpp create mode 100644 src/examples/jetdrive_control/jetdrive_control_params.c create mode 100644 src/examples/jetdrive_control/module.mk (limited to 'src') diff --git a/.gitignore b/.gitignore index 3e94cf620..63f0a70b7 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,9 @@ mavlink/include/mavlink/v0.9/ /Documentation/doxygen*objdb*tmp .tags .tags_sorted_by_file +*.creator +*.includes +*.files +*.config +*.user +*.autosave diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..ca740a79f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,7 @@ MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control #MODULES += examples/fixedwing_control +MODULES += examples/jetdrive_control # Hardware test #MODULES += examples/hwtest diff --git a/src/examples/jetdrive_control/jetdrive_control.h b/src/examples/jetdrive_control/jetdrive_control.h new file mode 100644 index 000000000..06c986092 --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control.h @@ -0,0 +1,207 @@ +#ifndef JETDRIVE_CONTROL_H +#define JETDRIVE_CONTROL_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int jetdrive_control_main(int argc, char *argv[]); + +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.05f +#define RATES_I_LIMIT 0.5f + +class JetdriveControl +{ +public: + /** + * Constructor + */ + JetdriveControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~JetdriveControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ + math::Matrix<3, 3> _R; /**< rotation matrix for current state */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; + param_t yaw_p; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for attitude setpoint updates. + */ + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + + +#ifndef jetdrive_control +#define jetdrive_control + +namespace jetdrive_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +JetdriveControl *g_control; +} + +#endif // jetdrive_control + +#endif // JETDRIVE_CONTROL_H diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp new file mode 100644 index 000000000..b66b9c214 --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -0,0 +1,678 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_main.c + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include "jetdrive_control.h" + + + +JetdriveControl::JetdriveControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _v_rates_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + + _R_sp.identity(); + _R.identity(); + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + I.identity(); + + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(); +} + +JetdriveControl::~JetdriveControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + jetdrive_control::g_control = nullptr; +} + +int +JetdriveControl::parameters_update() +{ + float v; + + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + return OK; +} + +void +JetdriveControl::parameter_update_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +JetdriveControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +void +JetdriveControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} + +void +JetdriveControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +JetdriveControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); + } +} + +void +JetdriveControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ +void +JetdriveControl::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; + + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } + + yaw_sp_move_rate *= _params.rc_scale_yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + _R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + } + + /* rotation matrix for current state */ + _R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = _R; + } + + /* R_rp and _R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(_R.transposed() * _R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +JetdriveControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } +} + +void +JetdriveControl::task_main_trampoline(int argc, char *argv[]) +{ + jetdrive_control::g_control->task_main(); +} + +void +JetdriveControl::task_main() +{ + warnx("started"); + fflush(stdout); + + /* + * do subscriptions + */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ + orb_set_interval(_v_att_sub, 5); + + /* initialize parameters cache */ + parameters_update(); + + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; + + fds[0].fd = _v_att_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + if (fds[0].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } + + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + } + + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +JetdriveControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("jetdrive_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&JetdriveControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int jetdrive_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: jetdrive_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (jetdrive_control::g_control != nullptr) + errx(1, "already running"); + + jetdrive_control::g_control = new JetdriveControl; + + if (jetdrive_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != jetdrive_control::g_control->start()) { + delete jetdrive_control::g_control; + jetdrive_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (jetdrive_control::g_control == nullptr) + errx(1, "not running"); + + delete jetdrive_control::g_control; + jetdrive_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (jetdrive_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/examples/jetdrive_control/jetdrive_control_params.c b/src/examples/jetdrive_control/jetdrive_control_params.c new file mode 100644 index 000000000..27a45b6bb --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. + */ + +#include + +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); diff --git a/src/examples/jetdrive_control/module.mk b/src/examples/jetdrive_control/module.mk new file mode 100644 index 000000000..2d1025451 --- /dev/null +++ b/src/examples/jetdrive_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 +# 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = jetdrive_control + +SRCS = jetdrive_control_main.cpp \ + jetdrive_control_params.c -- 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') 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') 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') 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') 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') 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 1acd951173212fcb3d7511792270609b748528d7 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:26:15 +0000 Subject: added publish to actuators when _v_control_mode.flag_external_manual_override_ok flag is set --- .../jetdrive_control/jetdrive_control_main.cpp | 77 ++++++++++++++-------- 1 file changed, 48 insertions(+), 29 deletions(-) (limited to 'src') diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index b66b9c214..d91f89401 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -558,45 +558,64 @@ JetdriveControl::task_main() arming_status_poll(); vehicle_manual_poll(); - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + if(_v_control_mode.flag_external_manual_override_ok) + { + /* manual/direct control */ + _actuators.control[0] = _manual_control_sp .roll; + _actuators.control[1] = _manual_control_sp.pitch; + _actuators.control[2] = _manual_control_sp.yaw; + _actuators.control[3] = _manual_control_sp.throttle; + _actuators.timestamp = hrt_absolute_time(); - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } - } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; - } + // FIX this: This is the atitude control logic for mc's - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } + + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } } -- 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') 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') 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 52b29cae39a93a0c5218e911e61d1f6cfe6115c1 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 14:21:34 +0000 Subject: added poll to 'attitude_rates_setpoint' as requested in TODO --- src/examples/jetdrive_control/jetdrive_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index d91f89401..409ccad63 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -596,8 +596,9 @@ JetdriveControl::task_main() } else { /* attitude controller disabled */ // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + vehicle_rates_setpoint_poll(); +// _rates_sp.zero(); +// _thrust_sp = 0.0f; } if (_v_control_mode.flag_control_rates_enabled) { -- 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') 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') 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 0789189c0588ebebd24a523f9639411be89c6a9b Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 28 Feb 2014 16:24:15 +0000 Subject: - Added JC params to jetdrive control example app. - fixed offboard control mode rates --- .../jetdrive_control/jetdrive_control_main.cpp | 35 ++++++++++++++-------- .../jetdrive_control/jetdrive_control_params.c | 27 +++++++++-------- 2 files changed, 36 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index 01435f572..bb72f14e0 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -95,19 +95,19 @@ JetdriveControl::JetdriveControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("JC_ROLL_P"); + _params_handles.roll_rate_p = param_find("JC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("JC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("JC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("JC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("JC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("JC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("JC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("JC_YAW_P"); + _params_handles.yaw_rate_p = param_find("JC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("JC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("JC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("JC_YAW_FF"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -463,6 +463,11 @@ JetdriveControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; + + // _att_control will be loaded to controls + // + // _att_control = (JC_XXXRATE_P * rates_err) + (JC_XXXRATE_D * (_rates_prev - rate) / dt) + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; @@ -597,6 +602,10 @@ JetdriveControl::task_main() /* attitude controller disabled */ // TODO poll 'attitude_rates_setpoint' topic vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; // _rates_sp.zero(); // _thrust_sp = 0.0f; } diff --git a/src/examples/jetdrive_control/jetdrive_control_params.c b/src/examples/jetdrive_control/jetdrive_control_params.c index 27a45b6bb..14831ecf7 100644 --- a/src/examples/jetdrive_control/jetdrive_control_params.c +++ b/src/examples/jetdrive_control/jetdrive_control_params.c @@ -41,16 +41,17 @@ #include -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +PARAM_DEFINE_FLOAT(JC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(JC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(JC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(JC_YAW_FF, 0.5f); -- 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') 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 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') 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') 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') 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') 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') 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') 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') 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') 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') 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 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') 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 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') 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 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') 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 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') 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') 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') 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 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') 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') 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') 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') 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 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') 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 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') 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 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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 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') 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 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') 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 ef6c99c1ab4ebce2378bfeef1813b6e9d01367ed Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 6 Jul 2014 11:40:28 -0700 Subject: Restructure the locking around SPI transfers --- src/drivers/device/spi.cpp | 62 ++++++++++++++++++++++------------------------ src/drivers/device/spi.h | 2 ++ 2 files changed, 32 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c..57cef34d2 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -133,26 +133,44 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t state; + int result; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; + LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; + /* lock the bus as required */ - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - state = irqsave(); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, true); - break; - case LOCK_NONE: - break; + switch (mode) { + default: + case LOCK_PREEMPTION: + { + irqstate_t state = irqsave(); + result = _transfer(send, recv, len); + irqrestore(state); } + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + result = _transfer(send, recv, len); + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + result = _transfer(send, recv, len); + break; } + return result; +} +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + +int +SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); SPI_SETBITS(_dev, 8); @@ -164,27 +182,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - irqrestore(state); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, false); - break; - case LOCK_NONE: - break; - } - } - return OK; } -void -SPI::set_frequency(uint32_t frequency) -{ - _frequency = frequency; -} - } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5..8e943b3f2 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,6 +129,8 @@ private: enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; } // namespace device -- 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') 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 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') 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 530e70bc4697a7e436a4cc9557ce38e139ee2795 Mon Sep 17 00:00:00 2001 From: Julian Oes 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') 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 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') 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') 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') 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') 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 c3826505ed36b4efa0272f863e45ea590b646e74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:15:20 +0200 Subject: HIL: reduce excessive stack usage of driver --- src/drivers/hil/hil.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 55cc077fb..f17e99e9d 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -229,7 +229,7 @@ HIL::init() _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, (main_t)&HIL::task_main_trampoline, nullptr); -- cgit v1.2.3 From 3b448078878b3b16b0145bc9c1288ce278c0acc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:15:47 +0200 Subject: Reduce excessive FMU stack usage --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728..d103935ae 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -329,7 +329,7 @@ PX4FMU::init() _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1600, (main_t)&PX4FMU::task_main_trampoline, nullptr); -- cgit v1.2.3 From 41f4e5a6bc3859d3e7682fafe01ad8a075121127 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:57:08 +0200 Subject: ardrone interface: Remove unused variable --- src/drivers/ardrone_interface/ardrone_motor_control.c | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 81f634992..fc017dd58 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -382,8 +382,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float output_band = 0.0f; const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - static bool initialized = false; - /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 -- 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') 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') 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 0054eb23d857844ebae34a7d198fc60e538ccd3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:44:22 +0200 Subject: Update sensors tests --- src/systemcmds/tests/test_sensors.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 096106ff3..a4f17eebd 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -139,7 +139,14 @@ accel(int argc, char *argv[]) } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("ACCEL1 acceleration values out of range!"); + warnx("ACCEL acceleration values out of range!"); + return ERROR; + } + + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL scale error!"); return ERROR; } @@ -186,6 +193,13 @@ accel1(int argc, char *argv[]) return ERROR; } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: ACCEL1 passed all tests successfully\n"); @@ -225,6 +239,13 @@ gyro(int argc, char *argv[]) printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); close(fd); @@ -263,6 +284,13 @@ gyro1(int argc, char *argv[]) printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO1 passed all tests successfully\n"); close(fd); @@ -301,6 +329,13 @@ mag(int argc, char *argv[]) printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 1.0f || len > 3.0f) { + warnx("MAG scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); close(fd); -- cgit v1.2.3 From 8c6745d53fcb75c14caa21f3cb917ab6fa5f2bcf Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 00:41:09 -0400 Subject: Prevent stack overflow when flashing px4io Large local variable causing stack overflow when attempting to flash IO!!! --- src/drivers/px4io/px4io_uploader.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 7b6361a7c..652949e41 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -413,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n) int PX4IO_Uploader::program(size_t fw_size) { - uint8_t file_buf[PROG_MULTI_MAX]; + uint8_t *file_buf; ssize_t count; int ret; size_t sent = 0; + file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + if (!file_buf) { + log("Can't allocate program buffer"); + return -ENOMEM; + } + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -438,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size) (int)errno); } - if (count == 0) + if (count == 0) { + free(file_buf); return OK; + } sent += count; @@ -455,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size) ret = get_sync(1000); - if (ret != OK) + if (ret != OK) { + free(file_buf); return ret; + } } + free(file_buf); return OK; } -- cgit v1.2.3 From 12da0efbb24ad135dbacb4b8f90199b3b38cb40d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 00:55:33 -0400 Subject: Read the full buffer sizeof wont work here since file_buf is now a pointer --- src/drivers/px4io/px4io_uploader.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 652949e41..d134c0246 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -432,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; } count = read_with_retry(_fw_fd, file_buf, n); -- cgit v1.2.3