Some mavlink fixes to use vehicle_control_mode, WIP

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;