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,81 +79,88 @@ private:
 			mavlink_hil_actuator_controls_t msg{};
 			msg.time_usec = act.timestamp;
 
-			static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
+			if (_use_dynamic_mixing) {
+				for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
+					msg.controls[i] = act.output[i];
+				}
 
-			unsigned system_type = _mavlink->get_system_type();
+			} else {
+				static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
 
-			/* scale outputs depending on system type */
-			if (system_type == MAV_TYPE_QUADROTOR ||
-			    system_type == MAV_TYPE_HEXAROTOR ||
-			    system_type == MAV_TYPE_OCTOROTOR ||
-			    system_type == MAV_TYPE_VTOL_DUOROTOR ||
-			    system_type == MAV_TYPE_VTOL_QUADROTOR ||
-			    system_type == MAV_TYPE_VTOL_RESERVED2) {
+				unsigned system_type = _mavlink->get_system_type();
 
-				/* multirotors: set number of rotor outputs depending on type */
+				/* scale outputs depending on system type */
+				if (system_type == MAV_TYPE_QUADROTOR ||
+				    system_type == MAV_TYPE_HEXAROTOR ||
+				    system_type == MAV_TYPE_OCTOROTOR ||
+				    system_type == MAV_TYPE_VTOL_DUOROTOR ||
+				    system_type == MAV_TYPE_VTOL_QUADROTOR ||
+				    system_type == MAV_TYPE_VTOL_RESERVED2) {
 
-				unsigned n;
+					/* multirotors: set number of rotor outputs depending on type */
 
-				switch (system_type) {
-				case MAV_TYPE_QUADROTOR:
-					n = 4;
-					break;
+					unsigned n;
 
-				case MAV_TYPE_HEXAROTOR:
-					n = 6;
-					break;
+					switch (system_type) {
+					case MAV_TYPE_QUADROTOR:
+						n = 4;
+						break;
 
-				case MAV_TYPE_VTOL_DUOROTOR:
-					n = 2;
-					break;
+					case MAV_TYPE_HEXAROTOR:
+						n = 6;
+						break;
 
-				case MAV_TYPE_VTOL_QUADROTOR:
-					n = 4;
-					break;
+					case MAV_TYPE_VTOL_DUOROTOR:
+						n = 2;
+						break;
 
-				case MAV_TYPE_VTOL_RESERVED2:
-					n = 8;
-					break;
+					case MAV_TYPE_VTOL_QUADROTOR:
+						n = 4;
+						break;
 
-				default:
-					n = 8;
-					break;
-				}
+					case MAV_TYPE_VTOL_RESERVED2:
+						n = 8;
+						break;
 
-				for (unsigned i = 0; i < 16; i++) {
-					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-						if (i < n) {
-							/* scale PWM out 900..2100 us to 0..1 for rotors */
-							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+					default:
+						n = 8;
+						break;
+					}
+
+					for (unsigned i = 0; i < 16; i++) {
+						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+							if (i < n) {
+								/* scale PWM out 900..2100 us to 0..1 for rotors */
+								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+
+							} else {
+								/* scale PWM out 900..2100 us to -1..1 for other channels */
+								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+							}
 
 						} else {
-							/* scale PWM out 900..2100 us to -1..1 for other channels */
-							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+							/* send 0 when disarmed and for disabled channels */
+							msg.controls[i] = 0.0f;
 						}
-
-					} else {
-						/* send 0 when disarmed and for disabled channels */
-						msg.controls[i] = 0.0f;
 					}
-				}
 
-			} else {
-				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
-				for (unsigned i = 0; i < 16; i++) {
-					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-						if (i != 3) {
-							/* scale PWM out 900..2100 us to -1..1 for normal channels */
-							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+				} else {
+					/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+					for (unsigned i = 0; i < 16; i++) {
+						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+							if (i != 3) {
+								/* scale PWM out 900..2100 us to -1..1 for normal channels */
+								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+
+							} else {
+								/* scale PWM out 900..2100 us to 0..1 for throttle */
+								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+							}
 
 						} else {
-							/* scale PWM out 900..2100 us to 0..1 for throttle */
-							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+							/* set 0 for disabled channels */
+							msg.controls[i] = 0.0f;
 						}
-
-					} else {
-						/* set 0 for disabled channels */
-						msg.controls[i] = 0.0f;
 					}
 				}
 			}

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,14 +276,28 @@ void Sih::read_motors()
 	float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
 
 	if (_actuator_out_sub.update(&actuators_out)) {
-		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]
-				_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
-
-			} else { // throttle signals in range [0,1]
-				float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
-				_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
+
+		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]
+					_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
+
+				} else { // throttle signals in range [0,1]
+					float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
+					_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
+				}
 			}
 		}
 	}

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,88 +99,96 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 
 	int _system_type = _param_mav_type.get();
 
-	/* '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;
-	bool is_fixed_wing;
-
-	switch (_system_type) {
-	case MAV_TYPE_AIRSHIP:
-	case MAV_TYPE_VTOL_DUOROTOR:
-	case MAV_TYPE_COAXIAL:
-		pos_thrust_motors_count = 2;
-		is_fixed_wing = false;
-		break;
+	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];
+			}
+		}
 
-	case MAV_TYPE_TRICOPTER:
-		pos_thrust_motors_count = 3;
-		is_fixed_wing = false;
-		break;
+	} 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;
+		bool is_fixed_wing;
+
+		switch (_system_type) {
+		case MAV_TYPE_AIRSHIP:
+		case MAV_TYPE_VTOL_DUOROTOR:
+		case MAV_TYPE_COAXIAL:
+			pos_thrust_motors_count = 2;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_QUADROTOR:
-	case MAV_TYPE_VTOL_QUADROTOR:
-	case MAV_TYPE_VTOL_TILTROTOR:
-		pos_thrust_motors_count = 4;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_TRICOPTER:
+			pos_thrust_motors_count = 3;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_VTOL_RESERVED2:
-		pos_thrust_motors_count = 5;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_QUADROTOR:
+		case MAV_TYPE_VTOL_QUADROTOR:
+		case MAV_TYPE_VTOL_TILTROTOR:
+			pos_thrust_motors_count = 4;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_HEXAROTOR:
-		pos_thrust_motors_count = 6;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_VTOL_RESERVED2:
+			pos_thrust_motors_count = 5;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_VTOL_RESERVED3:
-		// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
-		pos_thrust_motors_count = 3;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_HEXAROTOR:
+			pos_thrust_motors_count = 6;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_OCTOROTOR:
-		pos_thrust_motors_count = 8;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_VTOL_RESERVED3:
+			// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
+			pos_thrust_motors_count = 3;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_SUBMARINE:
-		pos_thrust_motors_count = 0;
-		is_fixed_wing = false;
-		break;
+		case MAV_TYPE_OCTOROTOR:
+			pos_thrust_motors_count = 8;
+			is_fixed_wing = false;
+			break;
 
-	case MAV_TYPE_FIXED_WING:
-		pos_thrust_motors_count = 0;
-		is_fixed_wing = true;
-		break;
+		case MAV_TYPE_SUBMARINE:
+			pos_thrust_motors_count = 0;
+			is_fixed_wing = false;
+			break;
 
-	default:
-		pos_thrust_motors_count = 0;
-		is_fixed_wing = false;
-		break;
-	}
+		case MAV_TYPE_FIXED_WING:
+			pos_thrust_motors_count = 0;
+			is_fixed_wing = true;
+			break;
+
+		default:
+			pos_thrust_motors_count = 0;
+			is_fixed_wing = false;
+			break;
+		}
 
-	for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
-		if (!armed) {
-			/* send 0 when disarmed and for disabled channels */
-			msg->controls[i] = 0.0f;
+		for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
+			if (!armed) {
+				/* send 0 when disarmed and for disabled channels */
+				msg->controls[i] = 0.0f;
 
-		} else if ((is_fixed_wing && i == 4) ||
-			   (!is_fixed_wing && i < pos_thrust_motors_count)) {	//multirotor, rotor channel
-			/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
-			msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-			msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);
+			} else if ((is_fixed_wing && i == 4) ||
+				   (!is_fixed_wing && i < pos_thrust_motors_count)) {	//multirotor, rotor channel
+				/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
+				msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+				msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);
 
-		} else {
-			const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
-			const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2;
+			} else {
+				const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
+				const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2;
 
-			/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
-			msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta;
-			msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f);
+				/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
+				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.
-	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
+	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