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/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; }