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

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;
 }
 
-/**
- * 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[])
+extern "C" __EXPORT 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();
-
-	virtual ~MulticopterRateControl();
+	MulticopterRateControl(bool vtol = false);
+	~MulticopterRateControl() override;
 
 	/** @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{};