From 3fdb082cb89dc82538637b3f060787d929105567 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 00:03:29 +0100 Subject: mavlink: AUTO states indication fixed --- src/modules/mavlink/mavlink.c | 47 ++++++++++++---------- src/modules/mavlink/orb_listener.c | 31 +++----------- src/modules/mavlink/orb_topics.h | 6 +-- src/modules/navigator/navigator_main.cpp | 27 +++++++++---- .../uORB/topics/position_setpoint_triplet.h | 3 ++ 5 files changed, 55 insertions(+), 59 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5406c35c2..4d975066f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + if (armed.armed) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -208,33 +207,37 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; union px4_custom_mode custom_mode; custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { + if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } 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; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ *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 get nav state - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - /* - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + if (pos_sp_triplet.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet.nav_state == NAV_STATE_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 7f6237535..69cd820a0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; +struct position_setpoint_triplet_s pos_sp_triplet; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -127,7 +127,6 @@ 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}, @@ -154,7 +153,6 @@ 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]); @@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet); /* enable or disable HIL */ if (v_status.hil_state == HIL_STATE_ON) @@ -682,26 +681,6 @@ 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) { @@ -777,9 +756,9 @@ 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 */ + /* --- POSITION SETPOINT TRIPLET --- */ + mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */ /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 30ba348cf..89f647bdc 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -94,7 +94,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; - int control_mode_sub; + int position_setpoint_triplet_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,8 +111,8 @@ 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; +/** Position setpoint triplet */ +extern struct position_setpoint_triplet_s pos_sp_triplet; /** RC channels */ extern struct rc_channels_s rc; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8985b28df..5a02bf522 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -189,6 +189,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + bool _pos_sp_triplet_updated; + char *nav_states_str[NAV_STATE_MAX]; struct { @@ -399,6 +401,7 @@ Navigator::Navigator() : _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _pos_sp_triplet_updated(false), _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); @@ -835,6 +838,11 @@ Navigator::task_main() } } + /* publish position setpoint triplet if updated */ + if (_pos_sp_triplet_updated) { + publish_position_setpoint_triplet(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); @@ -999,7 +1007,7 @@ Navigator::start_none() _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1018,7 +1026,7 @@ Navigator::start_ready() _rtl_state = RTL_STATE_NONE; } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1061,7 +1069,7 @@ Navigator::start_loiter() _pos_sp_triplet.next.valid = false; _mission_item_valid = false; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1174,7 +1182,7 @@ Navigator::set_mission_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1280,7 +1288,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1345,7 +1353,7 @@ Navigator::set_rtl_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1540,9 +1548,12 @@ Navigator::on_mission_item_reached() void Navigator::publish_position_setpoint_triplet() { - /* lazily publish the mission triplet only once available */ + /* update navigation state */ + _pos_sp_triplet.nav_state = static_cast(myState); + + /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the mission triplet */ + /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4b57833b6..e8d68281a 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,6 +45,7 @@ #include #include #include "../uORB.h" +#include /** * @addtogroup topics @@ -82,6 +83,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + nav_state_t nav_state; /**< navigation state */ }; /** -- cgit v1.2.3