fix(mixer_module): change MixingOutput to use float outputs (#26724)

* refactor(mixer_module): change MixingOutput to use float outputs

MixingOutput now passes float values to output drivers instead of
uint16_t. This removes the need for the 8192 offset encoding and
allows reversible motors to receive negative values directly.

* fix(mixer_module): fix float safety issues

-EscClient and voxl2_io: replace outputs[i] with fabs(outputs[i]) > 0.fto fix compilation issues
-GZMixingInterface: add explicit double cast to prevent compilation error
-PWMSim: replaced unit16 cast with lroundf given that now motors outputs can be negative and casting a negative float to unit16 is undefinder behaviour
-mixer_module: same fix of PWM (unit126 cast on negative float is undefined behaviour)

* refactor(mixer_module): float rounding suggestions

* fix(pwm_sim): fix inverted disarmed condition

* fix(mixer_module): more float rounding improvements

* fix(mixer_module_tests): use casting method which are now in drivers for rounding tests

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>

src/drivers/actuators/vertiq_io/vertiq_io.cpp

@@ -183,11 +183,11 @@ void VertiqIo::parameters_update()
 	}
 }
 
-void VertiqIo::OutputControls(uint16_t outputs[MAX_ACTUATORS])
+void VertiqIo::OutputControls(float outputs[MAX_ACTUATORS])
 {
 	//Put the mixer outputs into the output message
 	for (uint8_t i = 0; i < _transmission_message.num_cvs; i++) {
-		_transmission_message.commands[i] = outputs[i];
+		_transmission_message.commands[i] = static_cast<uint16_t>(lroundf(outputs[i]));
 	}
 
 	_operational_ifci.PackageIfciCommandsForTransmission(&_transmission_message, _output_message, &_output_len);
@@ -195,8 +195,7 @@ void VertiqIo::OutputControls(uint16_t outputs[MAX_ACTUATORS])
 	_serial_interface.ProcessSerialTx();
 }
 
-bool VertiqIo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			     unsigned num_control_groups_updated)
+bool VertiqIo::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 #ifdef CONFIG_USE_IFCI_CONFIGURATION
 

src/drivers/actuators/vertiq_io/vertiq_io.hpp

@@ -94,14 +94,13 @@ public:
 	void print_info();
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	/**
 	* @brief Used to package and transmit controls via IQUART
 	* @param outputs The output throttles calculated by the mixer
 	*/
-	void OutputControls(uint16_t outputs[MAX_ACTUATORS]);
+	void OutputControls(float outputs[MAX_ACTUATORS]);
 
 private:
 

src/drivers/actuators/voxl_esc/voxl_esc.cpp

@@ -1237,8 +1237,7 @@ void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
 }
 
 /* OutputModuleInterface */
-bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			    unsigned num_outputs, unsigned num_control_groups_updated)
+bool VoxlEsc::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	//in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function)
 	//So, if Run() is blocked by a custom command, this function will not be called until Run is running again
@@ -1247,9 +1246,16 @@ bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 		return false;
 	}
 
+	// Convert float outputs to uint16_t hardware values
+	uint16_t hw_outputs[VOXL_ESC_OUTPUT_CHANNELS] {};
+
+	for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
+		hw_outputs[i] = static_cast<uint16_t>(lroundf(outputs[i]));
+	}
+
 	// don't use mixed values... recompute now.
 	if (_turtle_mode_en) {
-		mix_turtle_mode(outputs);
+		mix_turtle_mode(hw_outputs);
 	}
 
 	for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
@@ -1259,24 +1265,24 @@ bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 		} else {
 			if ((_turtle_mode_en) || (_parameters.cmd_type == VOXL_ESC_RPM_CMDS)) {
 				if (_extended_rpm) {
-					if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; }
+					if (hw_outputs[i] > VOXL_ESC_RPM_MAX_EXT) { hw_outputs[i] = VOXL_ESC_RPM_MAX_EXT; }
 
 				} else {
-					if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; }
+					if (hw_outputs[i] > VOXL_ESC_RPM_MAX) { hw_outputs[i] = VOXL_ESC_RPM_MAX; }
 				}
 
 			} else if (_parameters.cmd_type == VOXL_ESC_PWM_CMDS) {
-				if (outputs[i] > VOXL_ESC_PWM_MAX) { outputs[i] = VOXL_ESC_PWM_MAX; }
+				if (hw_outputs[i] > VOXL_ESC_PWM_MAX) { hw_outputs[i] = VOXL_ESC_PWM_MAX; }
 
-				else if (outputs[i] < _min_active_pwm) { outputs[i] = _min_active_pwm; }
+				else if (hw_outputs[i] < _min_active_pwm) { hw_outputs[i] = _min_active_pwm; }
 			}
 
 			if (!_turtle_mode_en) {
-				_esc_chans[i].rate_req = outputs[i] * _output_map[i].direction;
+				_esc_chans[i].rate_req = hw_outputs[i] * _output_map[i].direction;
 
 			} else {
 				// mapping updated in mixTurtleMode, no remap needed here, but reverse direction
-				_esc_chans[i].rate_req = outputs[i] * _output_map[i].direction * (-1);
+				_esc_chans[i].rate_req = hw_outputs[i] * _output_map[i].direction * (-1);
 			}
 		}
 	}

src/drivers/actuators/voxl_esc/voxl_esc.hpp

@@ -83,8 +83,7 @@ public:
 	void print_params();
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	virtual int	init();
 	int device_init(); // function where uart port is opened and ESC queried

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -169,7 +169,7 @@ public:
 	{
 	}
 
-	void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	void update_outputs(float outputs[MAX_ACTUATORS], unsigned num_outputs)
 	{
 		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
 			return;
@@ -178,7 +178,7 @@ public:
 		uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS;
 
 		for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) {
-			if (outputs[i] != 0) {
+			if (fabsf(outputs[i]) > FLT_EPSILON) {
 				_max_number_of_nonzero_outputs = i + 1;
 				break;
 			}
@@ -187,7 +187,7 @@ public:
 		uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_];
 
 		for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) {
-			payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0);
+			payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.f); // Output is float16 so this would benefit rescaling to e.g. [-1,1]
 		}
 
 		const CanardTransferMetadata transfer_metadata = {

src/drivers/cyphal/Cyphal.cpp

@@ -435,8 +435,7 @@ void CyphalNode::sendPortList()
 	_uavcan_node_port_List_last = now;
 }
 
-bool UavcanMixingInterface::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool UavcanMixingInterface::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	// Note: This gets called from MixingOutput from within its update() function
 	// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect

src/drivers/cyphal/Cyphal.hpp

@@ -87,8 +87,7 @@ public:
 		  _node_mutex(node_mutex),
 		  _pub_manager(pub_manager) {}
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	void printInfo() { _mixing_output.printStatus(); }
 

src/drivers/dshot/DShot.cpp

@@ -374,8 +374,7 @@ void DShot::mixerChanged()
 	update_num_motors();
 }
 
-bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			  unsigned num_outputs, unsigned num_control_groups_updated)
+bool DShot::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (!_outputs_on) {
 		return false;
@@ -391,7 +390,7 @@ bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 
 	for (int i = 0; i < (int)num_outputs; i++) {
 
-		uint16_t output = outputs[i];
+		uint16_t output = static_cast<uint16_t>(lroundf(outputs[i]));
 
 		if (output == DSHOT_DISARM_VALUE) {
 

src/drivers/dshot/DShot.h

@@ -92,8 +92,7 @@ public:
 
 	bool telemetry_enabled() const { return _telemetry != nullptr; }
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
 

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -97,10 +97,15 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-bool LinuxPWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-				unsigned num_outputs, unsigned num_control_groups_updated)
+bool LinuxPWMOut::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	_pwm_out->send_output_pwm(outputs, num_outputs);
+	uint16_t hw_outputs[MAX_ACTUATORS] {};
+
+	for (unsigned i = 0; i < num_outputs; i++) {
+		hw_outputs[i] = static_cast<uint16_t>(lroundf(outputs[i]));
+	}
+
+	_pwm_out->send_output_pwm(hw_outputs, num_outputs);
 	return true;
 }
 

src/drivers/linux_pwm_out/linux_pwm_out.hpp

@@ -73,8 +73,7 @@ public:
 
 	int init();
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
 	static constexpr int MAX_ACTUATORS = 8;

src/drivers/pca9685_pwm_out/main.cpp

@@ -73,8 +73,7 @@ public:
 	static int custom_command(int argc, char *argv[]);
 	static int print_usage(const char *reason = nullptr);
 
-	bool updateOutputs(uint16_t *outputs, unsigned num_outputs,
-			   unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	int print_status() override;
 
@@ -155,8 +154,7 @@ int PCA9685Wrapper::init()
 	return PX4_OK;
 }
 
-bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs,
-				   unsigned num_control_groups_updated)
+bool PCA9685Wrapper::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (_state != STATE::RUNNING) { return false; }
 
@@ -164,11 +162,13 @@ bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs,
 	num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs;
 
 	for (uint8_t i = 0; i < num_outputs; ++i) {
+		uint16_t output = static_cast<uint16_t>(lroundf(outputs[i]));
+
 		if (param_duty_mode & (1 << i)) {
-			low_level_outputs[i] = outputs[i];
+			low_level_outputs[i] = output;
 
 		} else {
-			low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]);
+			low_level_outputs[i] = pca9685->calcRawFromPulse(output);
 		}
 	}
 

src/drivers/pwm_out/PWMOut.cpp

@@ -127,19 +127,18 @@ bool PWMOut::update_pwm_out_state(bool on)
 	return true;
 }
 
-bool PWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated)
+bool PWMOut::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	/* output to the servos */
 	if (_pwm_initialized) {
 		for (size_t i = 0; i < num_outputs; i++) {
 			if (!_mixing_output.isFunctionSet(i)) {
 				// do not run any signal on disabled channels
-				outputs[i] = 0;
+				outputs[i] = 0.f;
 			}
 
 			if (_pwm_mask & (1 << i)) {
-				up_pwm_servo_set(i, outputs[i]);
+				up_pwm_servo_set(i, static_cast<uint16_t>(lroundf(outputs[i])));
 			}
 		}
 	}

src/drivers/pwm_out/PWMOut.hpp

@@ -73,8 +73,7 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
 	void Run() override;

src/drivers/px4io/px4io.cpp

@@ -155,8 +155,7 @@ public:
 
 	uint16_t		system_status() const { return _status; }
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			   unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
 	void Run() override;
@@ -364,19 +363,22 @@ PX4IO::~PX4IO()
 	perf_free(_interface_write_perf);
 }
 
-bool PX4IO::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			  unsigned num_outputs, unsigned num_control_groups_updated)
+bool PX4IO::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
+	uint16_t hw_outputs[MAX_ACTUATORS] {};
+
 	for (size_t i = 0; i < num_outputs; i++) {
 		if (!_mixing_output.isFunctionSet(i)) {
 			// do not run any signal on disabled channels
 			outputs[i] = 0;
 		}
+
+		hw_outputs[i] = static_cast<uint16_t>(lroundf(outputs[i]));
 	}
 
 	if (!_test_fmu_fail) {
 		/* output to the servos */
-		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
+		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, hw_outputs, num_outputs);
 	}
 
 	return true;
@@ -504,7 +506,7 @@ void PX4IO::updateFailsafe()
 	uint16_t values[PX4IO_MAX_ACTUATORS] {};
 
 	for (unsigned i = 0; i < _max_actuators; i++) {
-		values[i] = _mixing_output.actualFailsafeValue(i);
+		values[i] = static_cast<uint16_t>(lroundf(_mixing_output.actualFailsafeValue(i)));
 	}
 
 	io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators);

src/drivers/roboclaw/Roboclaw.cpp

@@ -156,15 +156,10 @@ int Roboclaw::initializeUART()
 	}
 }
 
-bool Roboclaw::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			     unsigned num_outputs, unsigned num_control_groups_updated)
+bool Roboclaw::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f;
-	float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f;
-
-	setMotorSpeed(Motor::Right, right_motor_output);
-	setMotorSpeed(Motor::Left, left_motor_output);
-
+	setMotorSpeed(Motor::Right, (outputs[0] - 127.0f) / 127.f);
+	setMotorSpeed(Motor::Left, (outputs[1] - 127.0f) / 127.f);
 	return true;
 }
 
@@ -246,7 +241,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value)
 
 	// send command
 	if (motor == Motor::Right) {
-		if (value > 0) {
+		if (value > 0.f) {
 			command = Command::DriveForwardMotor1;
 
 		} else {
@@ -254,7 +249,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value)
 		}
 
 	} else if (motor == Motor::Left) {
-		if (value > 0) {
+		if (value > 0.f) {
 			command = Command::DriveForwardMotor2;
 
 		} else {
@@ -291,15 +286,9 @@ void Roboclaw::resetEncoders()
 	sendTransaction(Command::ResetEncoders, nullptr, 0);
 }
 
-void Roboclaw::sendUnsigned7Bit(Command command, float data)
+void Roboclaw::sendUnsigned7Bit(Command command, const float data)
 {
-	data = fabs(data);
-
-	if (data >= 1.0f) {
-		data = 0.99f;
-	}
-
-	auto byte = (uint8_t)(data * INT8_MAX);
+	uint8_t byte = static_cast<uint8_t>(lroundf(fabs(data) * INT8_MAX));
 	sendTransaction(command, &byte, 1);
 }
 

src/drivers/roboclaw/Roboclaw.hpp

@@ -79,8 +79,7 @@ public:
 	void Run() override;
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	void setMotorSpeed(Motor motor, float value); ///< rev/sec
 	void setMotorDutyCycle(Motor motor, float value);

src/drivers/roboclaw/module.yaml

@@ -10,9 +10,9 @@ actuator_output:
     - param_prefix: RBCLW
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 128, max: 128, default: 128 }
-        min: { min: 1, max: 128, default: 1 }
-        max: { min: 128, max: 256, default: 256 }
+        disarmed: { min: 127, max: 127, default: 127 }
+        min: { min: 0, max: 127, default: 0 }
+        max: { min: 127, max: 254, default: 254 }
         failsafe: { min: 0, max: 257 }
       num_channels: 2
 

src/drivers/tap_esc/TAP_ESC.cpp

@@ -243,7 +243,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet)
 	tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1);
 }
 
-bool TAP_ESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool TAP_ESC::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs,
 			    unsigned num_control_groups_updated)
 {
 	if (_initialized) {
@@ -252,26 +252,26 @@ bool TAP_ESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_output
 		// We need to remap from the system default to what PX4's normal scheme is
 		switch (num_outputs) {
 		case 4:
-			motor_out[0] = outputs[2];
-			motor_out[1] = outputs[1];
-			motor_out[2] = outputs[3];
-			motor_out[3] = outputs[0];
+			motor_out[0] = static_cast<uint16_t>(lroundf(outputs[2]));
+			motor_out[1] = static_cast<uint16_t>(lroundf(outputs[1]));
+			motor_out[2] = static_cast<uint16_t>(lroundf(outputs[3]));
+			motor_out[3] = static_cast<uint16_t>(lroundf(outputs[0]));
 			break;
 
 		case 6:
-			motor_out[0] = outputs[3];
-			motor_out[1] = outputs[0];
-			motor_out[2] = outputs[4];
-			motor_out[3] = outputs[2];
-			motor_out[4] = outputs[1];
-			motor_out[5] = outputs[5];
+			motor_out[0] = static_cast<uint16_t>(lroundf(outputs[3]));
+			motor_out[1] = static_cast<uint16_t>(lroundf(outputs[0]));
+			motor_out[2] = static_cast<uint16_t>(lroundf(outputs[4]));
+			motor_out[3] = static_cast<uint16_t>(lroundf(outputs[2]));
+			motor_out[4] = static_cast<uint16_t>(lroundf(outputs[1]));
+			motor_out[5] = static_cast<uint16_t>(lroundf(outputs[5]));
 			break;
 
 		default:
 
 			// Use the system defaults
 			for (uint8_t i = 0; i < num_outputs; ++i) {
-				motor_out[i] = outputs[i];
+				motor_out[i] = static_cast<uint16_t>(lroundf(outputs[i]));
 			}
 
 			break;

src/drivers/tap_esc/TAP_ESC.hpp

@@ -101,8 +101,7 @@ public:
 
 	int init();
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
 

src/drivers/uavcan/actuators/esc.cpp

@@ -85,7 +85,7 @@ int UavcanEscController::init()
 	return res;
 }
 
-void UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size)
+void UavcanEscController::update_outputs(float outputs[MAX_ACTUATORS], uint8_t output_array_size)
 {
 	// TODO: configurable rate limit
 	const auto timestamp = _node.getMonotonicTime();
@@ -99,7 +99,7 @@ void UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_
 	uavcan::equipment::esc::RawCommand msg{};
 
 	for (unsigned i = 0; i < output_array_size; i++) {
-		msg.cmd.push_back(static_cast<int>(outputs[i]));
+		msg.cmd.push_back(static_cast<int>(lroundf(outputs[i])));
 	}
 
 	_uavcan_pub_raw_cmd.broadcast(msg);

src/drivers/uavcan/actuators/esc.hpp

@@ -73,7 +73,7 @@ public:
 
 	bool initialized() { return _initialized; };
 
-	void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
+	void update_outputs(float outputs[MAX_ACTUATORS], uint8_t output_array_size);
 
 	/**
 	 * Sets the number of rotors and enable timer

src/drivers/uavcan/actuators/servo.cpp

@@ -44,8 +44,7 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) :
 	_uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
 }
 
-void
-UavcanServoController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+void UavcanServoController::update_outputs(float outputs[MAX_ACTUATORS], unsigned num_outputs)
 {
 	uavcan::equipment::actuator::ArrayCommand msg;
 
@@ -53,7 +52,7 @@ UavcanServoController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned
 		uavcan::equipment::actuator::Command cmd;
 		cmd.actuator_id = i;
 		cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
-		cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
+		cmd.command_value = outputs[i] / 500.f - 1.f; // TODO would benefit from [-1, 1] parameters
 
 		msg.commands.push_back(cmd);
 	}

src/drivers/uavcan/actuators/servo.hpp

@@ -52,7 +52,7 @@ public:
 	UavcanServoController(uavcan::INode &node);
 	~UavcanServoController() = default;
 
-	void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
+	void update_outputs(float outputs[MAX_ACTUATORS], unsigned num_outputs);
 
 private:
 	uavcan::INode								&_node;

src/drivers/uavcan/uavcan_main.cpp

@@ -1090,8 +1090,7 @@ void UavcanNode::publish_node_statuses()
 }
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool UavcanMixingInterfaceESC::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (_esc_controller.initialized()) {
 		// num_outputs is the maximum possible number of outputs (8)
@@ -1135,8 +1134,7 @@ void UavcanMixingInterfaceESC::mixerChanged()
 	_esc_controller.set_rotor_count(rotor_count);
 }
 
-bool UavcanMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool UavcanMixingInterfaceServo::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	_servo_controller.update_outputs(outputs, num_outputs);
 	return true;

src/drivers/uavcan/uavcan_main.hpp

@@ -129,8 +129,7 @@ public:
 		  _node_mutex(node_mutex),
 		  _esc_controller(esc_controller) {}
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	void mixerChanged() override;
 
@@ -162,8 +161,7 @@ public:
 		  _node_mutex(node_mutex),
 		  _servo_controller(servo_controller) {}
 
-	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	MixingOutput &mixingOutput() { return _mixing_output; }
 

src/drivers/voxl2_io/voxl2_io.cpp

@@ -301,8 +301,7 @@ int Voxl2IO::handle_uart_passthru()
 	return num_writes;
 }
 
-bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			    unsigned num_outputs, unsigned num_control_groups_updated)
+bool Voxl2IO::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	// Stop Mixer while ESCs are being calibrated
 	if (_outputs_disabled) {
@@ -326,11 +325,11 @@ bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
 			outputs[i] = 0;
 		}
 
-		if (outputs[i]) {
+		if (outputs[i] > 0.5f) {
 			_pwm_on = true;
 		}
 
-		output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE;  //convert to ns
+		output_cmds[i] = static_cast<uint32_t>(lroundf(outputs[i] * MIXER_OUTPUT_TO_CMD_SCALE)); //convert to ns
 	}
 
 	Command cmd;
@@ -347,9 +346,9 @@ bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
 
 	//if (_pwm_on && _debug){
 	if (_debug) {
-		PX4_INFO("Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]",
-			 outputs[0], outputs[1], outputs[2], outputs[3],
-			 outputs[4], outputs[5], outputs[6], outputs[7]);
+		PX4_INFO("Mixer outputs: [%.0f, %.0f, %.0f, %.0f, %.0f, %.0f, %.0f, %.0f]",
+			 (double)outputs[0], (double)outputs[1], (double)outputs[2], (double)outputs[3],
+			 (double)outputs[4], (double)outputs[5], (double)outputs[6], (double)outputs[7]);
 	}
 
 	perf_count(_output_update_perf);

src/drivers/voxl2_io/voxl2_io.hpp

@@ -85,8 +85,7 @@ public:
 	int print_status() override;
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	virtual int	init();