From fa20fae6fb627d19d9f3951e75058eb85ab8a10a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 22:38:22 +0400 Subject: [PATCH 1/3] Some mavlink fixes to use vehicle_control_mode, WIP --- src/modules/commander/commander.cpp | 39 ++++++++++++++++++++++++++--- src/modules/mavlink/mavlink.c | 20 ++++++--------- src/modules/mavlink/orb_listener.c | 27 ++++++++++++++++++++ src/modules/mavlink/orb_topics.h | 4 +++ 4 files changed, 75 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 199bfb0daf..98979df3e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -193,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -345,10 +345,11 @@ void print_status() static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; + bool ret = false; /* only handle high-priority commands here */ @@ -375,6 +376,8 @@ void 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; // TODO remove debug code //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); @@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) + ret = true; /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) + ret = true; if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); @@ -482,12 +490,36 @@ void 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 (?) + 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(); + mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE + status->set_nav_state = NAV_STATE_MISSION; + status->set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else { + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + } + } + break; + /* 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 transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { /* reject parachute depoyment not armed */ @@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &cmd, &armed); + if (handle_command(&status, &safety, &cmd, &armed)) + status_changed = true; } /* check which state machines for changes, clear "changed" flag */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4c38cf35a1..bc191f30d2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO use control_mode topic - /* - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + // TODO review + if (control_mode.nav_state == NAV_STATE_NONE) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 28478a8030..e168dded32 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; +struct vehicle_control_mode_s control_mode; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -125,6 +126,7 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); +static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -151,6 +153,7 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, + {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l) } +void +l_control_mode(const struct listener *l) +{ + orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); +} + static void * uorb_receive_thread(void *arg) { @@ -753,6 +776,10 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + /* --- CONTROL MODE --- */ + mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 9000728cbc..4d428406ac 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -95,6 +95,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; + int control_mode_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; +/** Vehicle control mode */ +extern struct vehicle_control_mode_s control_mode; + /** RC channels */ extern struct rc_channels_s rc; From 33385a783cec5045a910e4890fa8c8f4b2fc7641 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:04:24 +0400 Subject: [PATCH 2/3] Added NONE = not mapped state for mission and return switches --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/sensors/sensors.cpp | 24 ++++++++++++------------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 98979df3e1..03d3c02d15 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1392,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->mode_switch = MODE_SWITCH_ASSISTED; } - /* land switch */ + /* return switch */ if (!isfinite(sp_man->return_switch)) { current_status->return_switch = RETURN_SWITCH_NONE; @@ -1400,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ @@ -1416,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_MISSION; + current_status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_NONE; + current_status->mission_switch = MISSION_SWITCH_LOITER; } else { current_status->mission_switch = MISSION_SWITCH_MISSION; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c271..9baf1a6aff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1372,18 +1372,6 @@ Sensors::rc_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* mode switch input */ - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - - /* land switch input */ - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - - /* assisted switch input */ - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - - /* mission switch input */ - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1394,14 +1382,26 @@ Sensors::rc_poll() } } + /* mode switch input */ if (_rc.function[MODE] >= 0) { manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ if (_rc.function[MISSION] >= 0) { manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + 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); // } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ae3a24a1b7..1a9dec5f51 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -99,11 +99,13 @@ typedef enum { typedef enum { RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_NORMAL, RETURN_SWITCH_RETURN } return_switch_pos_t; typedef enum { MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_LOITER, MISSION_SWITCH_MISSION } mission_switch_pos_t; From c35c0a90d32e6a8003a136516b33056c9021ff32 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:04:55 +0400 Subject: [PATCH 3/3] navigator: use switches to select nav state --- src/modules/navigator/navigator_main.cpp | 106 ++++++++++++++++------- 1 file changed, 73 insertions(+), 33 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0a0ee25414..4176cee1b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -526,6 +526,8 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + unsigned prevState = 0; + /* wakeup source(s) */ struct pollfd fds[7]; @@ -545,7 +547,6 @@ Navigator::task_main() fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; - while (!_task_should_exit) { /* wait for up to 100ms for data */ @@ -571,41 +572,75 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_INIT: - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAV_STATE_RTL: + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { dispatch(EVENT_RTL_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* try mission, if none is available fallback to loiter instead */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + stick_mode = true; } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (!stick_mode) { + 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); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAV_STATE_RTL: + dispatch(EVENT_RTL_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } + + } else { + /* on first switch to AUTO try mission, if none is available fallback to loiter instead */ + if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; } } else { @@ -673,6 +708,11 @@ Navigator::task_main() } } + if (myState != prevState) { + mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + prevState = myState; + } + publish_control_mode(); perf_end(_loop_perf);