Merge pull request #1513 from PX4/isvtol

is_vtol flag in vehicle_status 

4 files changed, 49 insertions, 55 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ea3166051..dc0594bf2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1098,6 +1098,10 @@ int commander_thread_main(int argc, char *argv[])
 					status.is_rotary_wing = false;
 				}
 
+				/* set vehicle_status.is_vtol flag */
+				status.is_vtol =  (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
@@ -1119,6 +1123,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
+
+			/* Safety parameters */
 			param_get(_param_enable_parachute, &parachute_enabled);
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
@@ -1127,6 +1133,8 @@ 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);
+
+			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
 		}
 
@@ -2230,14 +2238,7 @@ set_control_mode()
 {
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
-	/* TODO: check this */
-	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_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 45dcddb9c..260b25a48 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -194,8 +194,6 @@ private:
 		float man_roll_max;						/**< Max Roll in rad */
 		float man_pitch_max;					/**< Max Pitch in rad */
 
-		param_t autostart_id;			/* indicates which airframe is used */
-
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -236,7 +234,6 @@ private:
 		param_t man_roll_max;
 		param_t man_pitch_max;
 
-		param_t autostart_id;		/* indicates which airframe is used */
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -338,6 +335,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators_0_pub(-1),
 	_actuators_2_pub(-1),
 
+	_rates_sp_id(0),
+	_actuators_id(0),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
 	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
@@ -396,20 +396,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
 
-	_parameter_handles.autostart_id 	= param_find("SYS_AUTOSTART");
-
 	/* fetch initial parameter values */
 	parameters_update();
-
-	/* set correct uORB ID, depending on if vehicle is VTOL or not */
-	if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
-		_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
-		_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-	}
-	else {
-		_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-		_actuators_id = ORB_ID(actuator_controls_0);
-	}
 }
 
 FixedwingAttitudeControl::~FixedwingAttitudeControl()
@@ -484,8 +472,6 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
-	param_get(_parameter_handles.autostart_id, &_parameters.autostart_id);
-
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -601,6 +587,16 @@ FixedwingAttitudeControl::vehicle_status_poll()
 
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
 	}
 }
 
@@ -701,11 +697,11 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
-			if (_parameters.autostart_id >= 13000
-			    && _parameters.autostart_id <= 13999) {	//vehicle type is VTOL, need to modify attitude!
-				/* The following modification to the attitude is vehicle specific and in this case applies
-				   to tail-sitter models !!!
-
+			if (_vehicle_status.is_vtol) {
+				/* vehicle type is VTOL, need to modify attitude!
+				 * The following modification to the attitude is vehicle specific and in this case applies
+				 *  to tail-sitter models !!!
+				 *
 				 * Since the VTOL airframe is initialized as a multicopter we need to
 				 * modify the estimated attitude for the fixed wing operation.
 				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
@@ -1031,7 +1027,7 @@ FixedwingAttitudeControl::task_main()
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude rates setpoint */
 					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
-				} else {
+				} else if (_rates_sp_id) {
 					/* advertise the attitude rates setpoint */
 					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
@@ -1056,7 +1052,7 @@ FixedwingAttitudeControl::task_main()
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
 				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else {
+			} else if (_actuators_id) {
 				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
 			}
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 0702e6378..82d2ff23a 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -126,8 +126,8 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
-	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
-	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure
+	orb_id_t _rates_sp_id;	/**< pointer to correct rates setpoint uORB metadata structure */
+	orb_id_t _actuators_id;	/**< pointer to correct actuator controls0 uORB metadata structure */
 
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
@@ -175,7 +175,6 @@ private:
 		param_t acro_pitch_max;
 		param_t acro_yaw_max;
 
-		param_t autostart_id;	//what frame are we using?
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -191,7 +190,6 @@ private:
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
 
-		param_t autostart_id;
 	}		_params;
 
 	/**
@@ -285,6 +283,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 	_actuators_0_circuit_breaker_enabled(false),
 
@@ -313,8 +313,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.man_yaw_max = 0.0f;
 	_params.acro_rate_max.zero();
 
-	_params.autostart_id = 0; //default
-
 	_rates_prev.zero();
 	_rates_sp.zero();
 	_rates_int.zero();
@@ -344,19 +342,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
 	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
 
-	_params_handles.autostart_id 	= param_find("SYS_AUTOSTART");
-
 	/* fetch initial parameter values */
 	parameters_update();
-	// set correct uORB ID, depending on if vehicle is VTOL or not
-	if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
-		_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
-		_actuators_id = ORB_ID(actuator_controls_virtual_mc);
-	}
-	else {
-		_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-		_actuators_id = ORB_ID(actuator_controls_0);
-	}
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -440,8 +427,6 @@ MulticopterAttitudeControl::parameters_update()
 
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
-	param_get(_params_handles.autostart_id, &_params.autostart_id);
-
 	return OK;
 }
 
@@ -531,6 +516,16 @@ MulticopterAttitudeControl::vehicle_status_poll()
 
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
 	}
 }
 
@@ -844,7 +839,7 @@ MulticopterAttitudeControl::task_main()
 				if (_v_rates_sp_pub > 0) {
 					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
 
-				} else {
+				} else if (_rates_sp_id) {
 					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
 				}
 
@@ -868,7 +863,7 @@ MulticopterAttitudeControl::task_main()
 					if (_v_rates_sp_pub > 0) {
 						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
 
-					} else {
+					} else if (_rates_sp_id) {
 						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
 					}
 
@@ -896,7 +891,7 @@ MulticopterAttitudeControl::task_main()
 					if (_actuators_0_pub > 0) {
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
 
-					} else {
+					} else if (_actuators_id) {
 						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
 					}
 
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 749d00d75..b4d5c7b88 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -185,7 +185,9 @@ struct vehicle_status_s {
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	bool is_rotary_wing;
+	bool is_rotary_wing;				/**< True if system is in rotary wing configuration, so for a VTOL
+							  this is only true while flying as a multicopter */
+	bool is_vtol;					/**< True if the system is VTOL capable */
 
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */