From fa20fae6fb627d19d9f3951e75058eb85ab8a10a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 22:38:22 +0400 Subject: 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 199bfb0da..98979df3e 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 4c38cf35a..bc191f30d 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 28478a803..e168dded3 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 9000728cb..4d428406a 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; -- cgit v1.2.3