hitl,sitl,sih: use separate actuator_outputs_sim for SYS_CTRL_ALLOC==1

- removes the need to do type-specific rescaling of pwm to normalized values
- allows to run physical output drivers alongside HIL/SIH

msg/actuator_outputs.msg

@@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS		= 16
 uint8 NUM_ACTUATOR_OUTPUT_GROUPS	= 4	# for sanity checking
 uint32 noutputs				# valid outputs
 float32[16] output				# output data, in natural output units
+
+# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
+# TOPICS actuator_outputs actuator_outputs_sim

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -81,9 +81,40 @@ bool
 PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		      unsigned num_control_groups_updated)
 {
-	// Nothing to do, as we are only interested in the actuator_outputs topic publication.
-	// That should only be published once we receive actuator_controls (important for lock-step to work correctly)
-	return num_control_groups_updated > 0;
+	// Only publish once we receive actuator_controls (important for lock-step to work correctly)
+	if (num_control_groups_updated > 0) {
+		actuator_outputs_s actuator_outputs{};
+		actuator_outputs.noutputs = num_outputs;
+
+		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
+
+		for (int i = 0; i < (int)num_outputs; i++) {
+			if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
+
+				OutputFunction function = _mixing_output.outputFunction(i);
+				bool is_reversible = reversible_outputs & (1u << i);
+				float output = outputs[i];
+
+				if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
+				    && !is_reversible) {
+					// Scale non-reversible motors to [0, 1]
+					actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
+
+				} else {
+					// Scale everything else to [-1, 1]
+					const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
+				}
+			}
+		}
+
+		actuator_outputs.timestamp = hrt_absolute_time();
+		_actuator_outputs_sim_pub.publish(actuator_outputs);
+		return true;
+	}
+
+	return false;
 }
 
 int

src/drivers/pwm_out_sim/PWMSim.hpp

@@ -43,6 +43,9 @@
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/time.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 
 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
 #define PARAM_PREFIX "PWM_MAIN"
@@ -86,5 +89,7 @@ private:
 
 	MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
 };
 

src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp

@@ -55,11 +55,21 @@ public:
 	}
 
 private:
-	explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+	explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{
+		int32_t sys_ctrl_alloc = 0;
+		param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
+		_use_dynamic_mixing = sys_ctrl_alloc >= 1;
+
+		if (_use_dynamic_mixing) {
+			_act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
+		}
+	}
 
 	uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	bool _use_dynamic_mixing{false};
 
 	bool send() override
 	{
@@ -69,6 +79,12 @@ private:
 			mavlink_hil_actuator_controls_t msg{};
 			msg.time_usec = act.timestamp;
 
+			if (_use_dynamic_mixing) {
+				for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
+					msg.controls[i] = act.output[i];
+				}
+
+			} else {
 				static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
 
 				unsigned system_type = _mavlink->get_system_type();
@@ -147,6 +163,7 @@ private:
 						}
 					}
 				}
+			}
 
 			// mode (MAV_MODE_FLAG)
 			msg.mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

src/modules/sih/sih.cpp

@@ -73,6 +73,10 @@ Sih::Sih() :
 	_dist_snsr_time = task_start;
 	_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
 					  static_cast<typeof _sih_vtype.get()>(2));
+
+	if (_sys_ctrl_alloc.get()) {
+		_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
+	}
 }
 
 Sih::~Sih()
@@ -99,6 +103,11 @@ bool Sih::init()
 
 void Sih::Run()
 {
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
 	perf_count(_loop_interval_perf);
 
 	// check for parameter updates
@@ -267,6 +276,19 @@ void Sih::read_motors()
 	float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
 
 	if (_actuator_out_sub.update(&actuators_out)) {
+
+		if (_sys_ctrl_alloc.get()) {
+			for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
+				if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) {
+					_u[i] = actuators_out.output[i];
+
+				} else {
+					float u_sp = actuators_out.output[i];
+					_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
+				}
+			}
+
+		} else {
 			for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
 				if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS
 						&& i > 3)) { // control surfaces in range [-1,1]
@@ -278,6 +300,7 @@ void Sih::read_motors()
 				}
 			}
 		}
+	}
 }
 
 // generate the motors thrust and torque in the body frame

src/modules/sih/sih.hpp

@@ -303,6 +303,7 @@ private:
 		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
 		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
 		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
-		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
+		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
+		(ParamBool<px4::params::SYS_CTRL_ALLOC>) _sys_ctrl_alloc
 	)
 };

src/modules/simulator/simulator.h

@@ -131,9 +131,7 @@ public:
 #endif
 
 private:
-	Simulator() : ModuleParams(nullptr)
-	{
-	}
+	Simulator();
 
 	~Simulator()
 	{
@@ -291,6 +289,7 @@ private:
 	float _last_magx{0.0f};
 	float _last_magy{0.0f};
 	float _last_magz{0.0f};
+	bool _use_dynamic_mixing{false};
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};

src/modules/simulator/simulator_mavlink.cpp

@@ -81,6 +81,14 @@ const unsigned mode_flag_custom = 1;
 
 using namespace time_literals;
 
+Simulator::Simulator()
+	: ModuleParams(nullptr)
+{
+	int32_t sys_ctrl_alloc = 0;
+	param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
+	_use_dynamic_mixing = sys_ctrl_alloc >= 1;
+}
+
 void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
 {
 	memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t));
@@ -91,6 +99,14 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 
 	int _system_type = _param_mav_type.get();
 
+	if (_use_dynamic_mixing) {
+		if (armed) {
+			for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
+				msg->controls[i] = _actuator_outputs.output[i];
+			}
+		}
+
+	} else {
 		/* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust)
 		all other motors are configured for -1..1 range */
 		unsigned pos_thrust_motors_count;
@@ -172,7 +188,7 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 				msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta;
 				msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f);
 			}
-
+		}
 	}
 
 	msg->mode = mode_flag_custom;
@@ -694,7 +710,12 @@ void Simulator::send()
 
 	// Subscribe to topics.
 	// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
+	if (_use_dynamic_mixing) {
+		_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0);
+
+	} else {
 		_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
+	}
 
 	// Before starting, we ought to send a heartbeat to initiate the SITL
 	// simulator to start sending sensor data which will set the time and