Some mavlink fixes to use vehicle_control_mode, WIP

3 files changed, 39 insertions, 12 deletions

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;