VTOL: explicitly start all FW & MC controllers in VTOL mode

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -15,16 +15,17 @@ ekf2 start
 #                        End Estimator group selection                        #
 ###############################################################################
 
-
-vtol_att_control start
-mc_rate_control start
-mc_att_control start
-mc_pos_control start
-fw_att_control start
-fw_pos_control_l1 start
 airspeed_selector start
 
-#
+vtol_att_control start
+
+mc_rate_control start vtol
+mc_att_control start vtol
+mc_pos_control start vtol
+
+fw_att_control start vtol
+fw_pos_control_l1 start vtol
+
 # Start Land Detector
 # Multicopter for now until we have something for VTOL
 #

Tools/uorb_graph/create.py

@@ -239,15 +239,6 @@ class Graph(object):
 
         special_cases_pub = [
     ('replay', r'Replay\.cpp$', None, r'^sub\.orb_meta$'),
-    ('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
-
-    ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
-
-    ('mc_rate_control', r'MulticopterRateControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
-    ('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'),
-
-    ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
-    ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
 
     ('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'),
     ]

posix-configs/SITL/init/test/test_shutdown

@@ -26,11 +26,11 @@ land_detector start vtol
 navigator start
 ekf2 start
 vtol_att_control start
-mc_pos_control start
+mc_pos_control start vtol
-mc_att_control start
+mc_att_control start vtol
-mc_rate_control start
+mc_rate_control start vtol
-fw_pos_control_l1 start
+fw_pos_control_l1 start vtol
-fw_att_control start
+fw_att_control start vtol
 airspeed_selector start
 
 #mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -40,13 +40,21 @@ using math::constrain;
 using math::gradual;
 using math::radians;
 
-FixedwingAttitudeControl::FixedwingAttitudeControl() :
+FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
+	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 	// check if VTOL first
-	vehicle_status_poll();
+	if (vtol) {
+		int32_t vt_type = -1;
+
+		if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+			_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
+		}
+	}
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -165,14 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 
 					_att_sp.timestamp = hrt_absolute_time();
 
-					if (_attitude_sp_pub != nullptr) {
+					_attitude_sp_pub.publish(_att_sp);
-						/* publish the attitude rates setpoint */
-						orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
-
-					} else if (_attitude_setpoint_id) {
-						/* advertise the attitude rates setpoint */
-						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
-					}
 
 				} else if (_vcontrol_mode.flag_control_rates_enabled &&
 					   !_vcontrol_mode.flag_control_attitude_enabled) {
@@ -220,30 +221,6 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_status_poll()
-{
-	if (_vehicle_status_sub.update(&_vehicle_status)) {
-		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (!_actuators_id) {
-			if (_vehicle_status.is_vtol) {
-				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
-
-				int32_t vt_type = -1;
-
-				if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
-					_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
-				}
-
-			} else {
-				_actuators_id = ORB_ID(actuator_controls_0);
-				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
-			}
-		}
-	}
-}
-
 void
 FixedwingAttitudeControl::vehicle_land_detected_poll()
 {
@@ -378,7 +355,10 @@ void FixedwingAttitudeControl::Run()
 		const matrix::Eulerf euler_angles(R);
 
 		vehicle_attitude_setpoint_poll();
-		vehicle_status_poll(); // this poll has to be before the control_mode_poll, otherwise rate sp are not published during whole transition
+
+		// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
+		_vehicle_status_sub.update(&_vehicle_status);
+
 		vehicle_control_mode_poll();
 		vehicle_manual_poll();
 		_global_pos_sub.update(&_global_pos);
@@ -646,14 +626,8 @@ void FixedwingAttitudeControl::Run()
 		if (_vcontrol_mode.flag_control_rates_enabled ||
 		    _vcontrol_mode.flag_control_attitude_enabled ||
 		    _vcontrol_mode.flag_control_manual_enabled) {
-			/* publish the actuator controls */
-			if (_actuators_0_pub != nullptr) {
-				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-
-			} else if (_actuators_id) {
-				_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
-			}
 
+			_actuators_0_pub.publish(_actuators);
 			_actuators_2_pub.publish(_actuators_airframe);
 		}
 	}
@@ -728,7 +702,15 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 
 int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
 {
-	FixedwingAttitudeControl *instance = new FixedwingAttitudeControl();
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol);
 
 	if (instance) {
 		_object.store(instance);
@@ -774,8 +756,9 @@ fw_att_control is the fixed wing attitude controller.
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_NAME("fw_att_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -76,7 +76,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	public px4::WorkItem
 {
 public:
-	FixedwingAttitudeControl();
+	FixedwingAttitudeControl(bool vtol = false);
 	~FixedwingAttitudeControl() override;
 
 	/** @see ModuleBase */
@@ -112,15 +112,11 @@ private:
 
 	uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
+	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
 	uORB::Publication<actuator_controls_s>		_actuators_2_pub{ORB_ID(actuator_controls_2)};		/**< actuator control group 1 setpoint (Airframe) */
-	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};		/**< rate setpoint publication */
+	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub;
-	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};	/**< rate controller status publication */
+	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
-
+	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
-	orb_id_t	_attitude_setpoint_id{nullptr};
-	orb_advert_t	_attitude_sp_pub{nullptr};	/**< attitude setpoint point */
-
-	orb_id_t	_actuators_id{nullptr};		/**< pointer to correct actuator controls0 uORB metadata structure */
-	orb_advert_t	_actuators_0_pub{nullptr};	/**< actuator control group 0 setpoint */
 
 	actuator_controls_s			_actuators {};		/**< actuator control inputs */
 	actuator_controls_s			_actuators_airframe {};	/**< actuator control inputs */
@@ -233,7 +229,6 @@ private:
 	void		vehicle_manual_poll();
 	void		vehicle_attitude_setpoint_poll();
 	void		vehicle_rates_setpoint_poll();
-	void		vehicle_status_poll();
 	void		vehicle_land_detected_poll();
 
 	float 		get_airspeed_and_update_scaling();

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -33,13 +33,29 @@
 
 #include "FixedwingPositionControl.hpp"
 
-FixedwingPositionControl::FixedwingPositionControl() :
+#include <vtol_att_control/vtol_type.h>
+
+FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw_pos_control_l1: cycle")),
+	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_launchDetector(this),
 	_runway_takeoff(this)
 {
+	if (vtol) {
+		_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
+
+		// VTOL parameter VTOL_TYPE
+		int32_t vt_type = -1;
+		param_get(param_find("VT_TYPE"), &vt_type);
+
+		_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
+
+	} else {
+		_parameter_handles.airspeed_trans = PARAM_INVALID;
+	}
+
 	// limit to 50 Hz
 	_global_pos_sub.set_interval_ms(20);
 
@@ -98,13 +114,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 
-	// if vehicle is vtol these handles will be set when we get the vehicle status
-	_parameter_handles.airspeed_trans = PARAM_INVALID;
-	_parameter_handles.vtol_type = PARAM_INVALID;
-
-	// initialize to invalid vtol type
-	_parameters.vtol_type = -1;
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -166,11 +175,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale));
 	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 
-	// VTOL parameter VTOL_TYPE
-	if (_parameter_handles.vtol_type != PARAM_INVALID) {
-		param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
-	}
-
 	// VTOL parameter VT_ARSP_TRANS
 	if (_parameter_handles.airspeed_trans != PARAM_INVALID) {
 		param_get(_parameter_handles.airspeed_trans, &_parameters.airspeed_trans);
@@ -333,27 +337,6 @@ FixedwingPositionControl::vehicle_command_poll()
 	}
 }
 
-void
-FixedwingPositionControl::vehicle_status_poll()
-{
-	if (_vehicle_status_sub.update(&_vehicle_status)) {
-		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_attitude_setpoint_id == nullptr) {
-			if (_vehicle_status.is_vtol) {
-				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
-
-				_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
-				_parameter_handles.vtol_type = param_find("VT_TYPE");
-
-				parameters_update();
-
-			} else {
-				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
-			}
-		}
-	}
-}
-
 void
 FixedwingPositionControl::airspeed_poll()
 {
@@ -399,12 +382,12 @@ FixedwingPositionControl::vehicle_attitude_poll()
 
 		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
 		// between multirotor and fixed wing flight
-		if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
+		if (_vtol_tailsitter) {
 			Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
 			_R_nb = _R_nb * R_offset;
 		}
 
-		Eulerf euler_angles(_R_nb);
+		const Eulerf euler_angles(_R_nb);
 		_roll    = euler_angles(0);
 		_pitch   = euler_angles(1);
 		_yaw     = euler_angles(2);
@@ -1708,7 +1691,7 @@ FixedwingPositionControl::Run()
 		vehicle_command_poll();
 		vehicle_control_mode_poll();
 		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
-		vehicle_status_poll();
+		_vehicle_status_sub.update(&_vehicle_status);
 		_vehicle_acceleration_sub.update();
 		_vehicle_rates_sub.update();
 
@@ -1753,15 +1736,7 @@ FixedwingPositionControl::Run()
 			    _control_mode.flag_control_acceleration_enabled ||
 			    _control_mode.flag_control_altitude_enabled) {
 
-				/* lazily publish the setpoint only once available */
+				_attitude_sp_pub.publish(_att_sp);
-				if (_attitude_sp_pub != nullptr) {
-					/* publish the attitude setpoint */
-					orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
-
-				} else if (_attitude_setpoint_id != nullptr) {
-					/* advertise and publish */
-					_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
-				}
 
 				// only publish status in full FW mode
 				if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
@@ -1898,7 +1873,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 
 	// tailsitters use the multicopter frame as reference, in fixed wing
 	// we need to use the fixed wing frame
-	if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
+	if (_vtol_tailsitter) {
 		float tmp = accel_body(0);
 		accel_body(0) = -accel_body(2);
 		accel_body(2) = tmp;
@@ -1944,7 +1919,15 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 
 int FixedwingPositionControl::task_spawn(int argc, char *argv[])
 {
-	FixedwingPositionControl *instance = new FixedwingPositionControl();
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	FixedwingPositionControl *instance = new FixedwingPositionControl(vtol);
 
 	if (instance) {
 		_object.store(instance);
@@ -1990,9 +1973,9 @@ fw_pos_control_l1 is the fixed wing position controller.
 
 )DESCR_STR");
 
-
 	PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -129,7 +129,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	public px4::WorkItem
 {
 public:
-	FixedwingPositionControl();
+	FixedwingPositionControl(bool vtol = false);
 	~FixedwingPositionControl() override;
 
 	/** @see ModuleBase */
@@ -165,9 +165,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			///< vehicle status subscription
 	uORB::SubscriptionData<vehicle_angular_velocity_s>	_vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
 
-	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint
+	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
-	orb_id_t	_attitude_setpoint_id{nullptr};
-
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
 	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
@@ -253,6 +251,8 @@ private:
 	float _asp_after_transition{0.0f};
 	bool _was_in_transition{false};
 
+	bool _vtol_tailsitter{false};
+
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
 	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
@@ -307,7 +307,6 @@ private:
 
 		// VTOL
 		float airspeed_trans;
-		int32_t vtol_type;
 	} _parameters{};					///< local copies of interesting parameters
 
 	struct {
@@ -370,8 +369,6 @@ private:
 		param_t land_airspeed_scale;
 		param_t land_throtTC_scale;
 		param_t loiter_radius;
-
-		param_t vtol_type;
 	} _parameter_handles {};				///< handles for interesting parameters
 
 	DEFINE_PARAMETERS(

src/modules/mc_att_control/mc_att_control.hpp

@@ -66,9 +66,8 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	public px4::WorkItem
 {
 public:
-	MulticopterAttitudeControl();
+	MulticopterAttitudeControl(bool vtol = false);
-
+	~MulticopterAttitudeControl() override;
-	virtual ~MulticopterAttitudeControl();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -93,8 +92,6 @@ private:
 	 */
 	void		parameters_updated();
 
-	void		vehicle_status_poll();
-
 	void		publish_rates_setpoint();
 
 	float		throttle_curve(float throttle_stick_input);
@@ -122,10 +119,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
 
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
-
+	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
-	orb_advert_t	_vehicle_attitude_setpoint_pub{nullptr};
-
-	orb_id_t _attitude_sp_id{nullptr};	/**< pointer to correct attitude setpoint uORB metadata structure */
 
 	struct vehicle_attitude_s		_v_att {};		/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s	_v_att_sp {};		/**< vehicle attitude setpoint */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -51,11 +51,20 @@
 
 using namespace matrix;
 
-MulticopterAttitudeControl::MulticopterAttitudeControl() :
+MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
+	if (vtol) {
+		int32_t vt_type = -1;
+
+		if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+			_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
+		}
+	}
+
 	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 
 	/* initialize quaternions in messages to be valid */
@@ -95,29 +104,6 @@ MulticopterAttitudeControl::parameters_updated()
 	_man_tilt_max = math::radians(_param_mpc_man_tilt_max.get());
 }
 
-void
-MulticopterAttitudeControl::vehicle_status_poll()
-{
-	/* check if there is new status information */
-	if (_vehicle_status_sub.update(&_vehicle_status)) {
-		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_attitude_sp_id == nullptr) {
-			if (_vehicle_status.is_vtol) {
-				_attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);
-
-				int32_t vt_type = -1;
-
-				if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
-					_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
-				}
-
-			} else {
-				_attitude_sp_id = ORB_ID(vehicle_attitude_setpoint);
-			}
-		}
-	}
-}
-
 float
 MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
@@ -233,9 +219,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
 	attitude_setpoint.timestamp = hrt_absolute_time();
 
-	if (_attitude_sp_id != nullptr) {
+	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
-		orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
-	}
 }
 
 /**
@@ -308,7 +292,7 @@ MulticopterAttitudeControl::Run()
 		_manual_control_sp_sub.update(&_manual_control_sp);
 		_v_control_mode_sub.update(&_v_control_mode);
 		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
-		vehicle_status_poll();
+		_vehicle_status_sub.update(&_vehicle_status);
 
 		/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
 		* or roll (yaw can rotate 360 in normal att control). If both are true don't
@@ -365,7 +349,15 @@ MulticopterAttitudeControl::Run()
 
 int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
 {
-	MulticopterAttitudeControl *instance = new MulticopterAttitudeControl();
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol);
 
 	if (instance) {
 		_object.store(instance);
@@ -425,6 +417,7 @@ https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth
 
 	PRINT_MODULE_USAGE_NAME("mc_att_control", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -83,8 +83,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	public ModuleParams, public px4::WorkItem
 {
 public:
-	MulticopterPositionControl();
+	MulticopterPositionControl(bool vtol = false);
-
 	~MulticopterPositionControl() override;
 
 	/** @see ModuleBase */
@@ -106,8 +105,7 @@ public:
 private:
 	Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
 
-	orb_id_t _attitude_setpoint_id{nullptr}; ///< orb metadata to publish attitude setpoint dependent if VTOL or not
+	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
-	orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; ///< attitude setpoint publication handle
 	uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	 /**< vehicle command publication */
 	orb_advert_t _mavlink_log_pub{nullptr};
 
@@ -236,11 +234,6 @@ private:
 	 */
 	void warn_rate_limited(const char *str);
 
-	/**
-	 * Publish attitude.
-	 */
-	void publish_attitude();
-
 	/**
 	 * Adjust the setpoint during landing.
 	 * Thrust is adjusted to support the land-detector during detection.
@@ -280,15 +273,21 @@ private:
 	void send_vehicle_cmd_do(uint8_t nav_state);
 };
 
-MulticopterPositionControl::MulticopterPositionControl() :
+MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
 	SuperBlock(nullptr, "MPC"),
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_vel_x_deriv(this, "VELD"),
 	_vel_y_deriv(this, "VELD"),
 	_vel_z_deriv(this, "VELD"),
 	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
 {
+	if (vtol) {
+		// if vehicle is a VTOL we want to enable weathervane capabilities
+		_wv_controller = new WeatherVane();
+	}
+
 	// fetch initial parameter values
 	parameters_update(true);
 
@@ -394,24 +393,7 @@ MulticopterPositionControl::parameters_update(bool force)
 void
 MulticopterPositionControl::poll_subscriptions()
 {
-	if (_vehicle_status_sub.update(&_vehicle_status)) {
+	_vehicle_status_sub.update(&_vehicle_status);
-
-		// set correct uORB ID, depending on if vehicle is VTOL or not
-		if (!_attitude_setpoint_id) {
-			if (_vehicle_status.is_vtol) {
-				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
-
-			} else {
-				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
-			}
-		}
-
-		// if vehicle is a VTOL we want to enable weathervane capabilities
-		if (_wv_controller == nullptr && _vehicle_status.is_vtol) {
-			_wv_controller = new WeatherVane();
-		}
-	}
-
 	_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 	_control_mode_sub.update(&_control_mode);
 	_home_pos_sub.update(&_home_pos);
@@ -702,9 +684,7 @@ MulticopterPositionControl::Run()
 			// Not publishing when not running a flight task
 			// in stabilized mode attitude setpoints get ignored
 			// in offboard with attitude setpoints they come from MAVLink directly
-			if (_attitude_setpoint_id != nullptr) {
+			_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
-				orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
-			}
 
 			_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
 
@@ -1079,7 +1059,15 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
 
 int MulticopterPositionControl::task_spawn(int argc, char *argv[])
 {
-	MulticopterPositionControl *instance = new MulticopterPositionControl();
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	MulticopterPositionControl *instance = new MulticopterPositionControl(vtol);
 
 	if (instance) {
 		_object.store(instance);
@@ -1124,6 +1112,7 @@ logging.
 
 	PRINT_MODULE_USAGE_NAME("mc_pos_control", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -42,9 +42,10 @@ using namespace matrix;
 using namespace time_literals;
 using math::radians;
 
-MulticopterRateControl::MulticopterRateControl() :
+MulticopterRateControl::MulticopterRateControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@@ -97,23 +98,6 @@ MulticopterRateControl::parameters_updated()
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
 }
 
-void
-MulticopterRateControl::vehicle_status_poll()
-{
-	/* check if there is new status information */
-	if (_vehicle_status_sub.update(&_vehicle_status)) {
-		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_actuators_id == nullptr) {
-			if (_vehicle_status.is_vtol) {
-				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
-
-			} else {
-				_actuators_id = ORB_ID(actuator_controls_0);
-			}
-		}
-	}
-}
-
 float
 MulticopterRateControl::get_landing_gear_state()
 {
@@ -183,7 +167,7 @@ MulticopterRateControl::Run()
 			}
 		}
 
-		vehicle_status_poll();
+		_vehicle_status_sub.update(&_vehicle_status);
 
 		const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
 
@@ -326,14 +310,14 @@ MulticopterRateControl::Run()
 			}
 
 			actuators.timestamp = hrt_absolute_time();
-			orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+			_actuators_0_pub.publish(actuators);
 
 		} else if (_v_control_mode.flag_control_termination_enabled) {
 			if (!_vehicle_status.is_vtol) {
 				// publish actuator controls
 				actuator_controls_s actuators{};
 				actuators.timestamp = hrt_absolute_time();
-				orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+				_actuators_0_pub.publish(actuators);
 			}
 		}
 	}
@@ -343,7 +327,15 @@ MulticopterRateControl::Run()
 
 int MulticopterRateControl::task_spawn(int argc, char *argv[])
 {
-	MulticopterRateControl *instance = new MulticopterRateControl();
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	MulticopterRateControl *instance = new MulticopterRateControl(vtol);
 
 	if (instance) {
 		_object.store(instance);
@@ -394,19 +386,15 @@ The controller has a PID loop for angular rate error.
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
 }
 
-/**
+extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[])
- * Multicopter rate control app start / stop handling function
- */
-extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
-
-int mc_rate_control_main(int argc, char *argv[])
 {
 	return MulticopterRateControl::main(argc, argv);
 }

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -63,9 +63,8 @@
 class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
 {
 public:
-	MulticopterRateControl();
+	MulticopterRateControl(bool vtol = false);
-
+	~MulticopterRateControl() override;
-	virtual ~MulticopterRateControl();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -90,8 +89,6 @@ private:
 	 */
 	void		parameters_updated();
 
-	void		vehicle_status_poll();
-
 	/**
 	 * Get the landing gear state based on the manual control switch position
 	 * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
@@ -112,13 +109,11 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
 
+	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
 	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
 	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
 
-	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
-	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
-
 	landing_gear_s 			_landing_gear{};
 	manual_control_setpoint_s	_manual_control_sp{};
 	vehicle_control_mode_s		_v_control_mode{};