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/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp

@@ -62,8 +62,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
 	return true;
 }
 
-bool GZMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool GZMixingInterfaceESC::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	unsigned active_output_count = 0;
 

src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp

@@ -55,8 +55,7 @@ public:
 		_node(node)
 	{}
 
-	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/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp

@@ -130,8 +130,7 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
 	return true;
 }
 
-bool GZMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool GZMixingInterfaceServo::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	bool updated = false;
 	// cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
@@ -142,8 +141,8 @@ bool GZMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsi
 		if (_mixing_output.isFunctionSet(i)) {
 			gz::msgs::Double servo_output;
 
-			double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i);
-			double output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_range;
+			double output_range = (double)_mixing_output.maxValue(i) - (double)_mixing_output.minValue(i);
+			double output = _angle_min_rad[i] + _angular_range_rad[i] * ((double)outputs[i] - (double)_mixing_output.minValue(i)) / output_range;
 			// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
 			// std::cout << "  output: " << output << std::endl;
 			servo_output.set_data(output);

src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp

@@ -49,8 +49,7 @@ public:
 		_node(node)
 	{}
 
-	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/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

@@ -60,8 +60,7 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name)
 	return true;
 }
 
-bool GZMixingInterfaceWheel::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		unsigned num_control_groups_updated)
+bool GZMixingInterfaceWheel::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	unsigned active_output_count = 0;
 

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

@@ -56,8 +56,7 @@ public:
 		_node(node)
 	{}
 
-	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/modules/simulation/pwm_out_sim/PWMSim.cpp

@@ -60,8 +60,7 @@ PWMSim::~PWMSim()
 	perf_free(_interval_perf);
 }
 
-bool PWMSim::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			   unsigned num_control_groups_updated)
+bool PWMSim::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	// Only publish once we receive actuator_controls (important for lock-step to work correctly)
 	if (num_control_groups_updated > 0) {
@@ -71,7 +70,7 @@ bool PWMSim::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned 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) {
+			if (fabsf(outputs[i] - PWM_SIM_DISARMED_MAGIC) > FLT_EPSILON) {
 
 				OutputFunction function = _mixing_output.outputFunction(i);
 				bool is_reversible = reversible_outputs & (1u << i);

src/modules/simulation/pwm_out_sim/PWMSim.hpp

@@ -72,8 +72,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;