let commander know if VTOL is in fw or in mc mode (important because of external_override)

1 files changed, 38 insertions, 3 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c9c8d16b5..bed467698 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -82,6 +82,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -149,6 +150,9 @@ enum MAV_MODE_FLAG {
 /* Mavlink file descriptors */
 static int mavlink_fd = 0;
 
+/* Syste autostart ID */
+static int autostart_id;
+
 /* flags */
 static bool commander_initialized = false;
 static volatile bool thread_should_exit = false;		/**< daemon exit flag */
@@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
 	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
+	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
 	/* welcome user */
 	warnx("starting");
@@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 
+	/* Subscribe to vtol vehicle status topic */
+	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
+	struct vtol_vehicle_status_s vtol_status;
+	memset(&vtol_status, 0, sizeof(vtol_status));
+	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
+
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
+
 					status.is_rotary_wing = true;
 
 				} else {
@@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
+			param_get(_param_autostart_id, &autostart_id);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* update vtol vehicle status*/
+		orb_check(vtol_vehicle_status_sub, &updated);
+
+		if (updated) {
+			/* vtol status changed */
+			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
+
+			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
+			if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
+				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
+			}
+		}
+
 		/* update global position estimate */
 		orb_check(global_position_sub, &updated);
 
@@ -2189,7 +2218,13 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	/* TODO: check this */
-	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	if (autostart_id < 13000 || autostart_id >= 14000) {
+		control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+
+	} else {
+		control_mode.flag_external_manual_override_ok = false;
+	}
+
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
@@ -2205,7 +2240,7 @@ set_control_mode()
 		control_mode.flag_control_velocity_enabled = false;
 		control_mode.flag_control_termination_enabled = false;
 		break;
-
+		
 	case NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;