diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.cpp b/src/drivers/actuators/vertiq_io/vertiq_io.cpp index 10a9dd1a18..bdef50b5f2 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.cpp +++ b/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(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 diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.hpp b/src/drivers/actuators/vertiq_io/vertiq_io.hpp index adbf5f9e16..eb0079dc43 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.hpp +++ b/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: diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 1d0b6114c3..002cba3b2c 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/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(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); } } } diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index d9e02c6f0f..450c15bd1a 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/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 diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index b6f469c90e..4a698ba423 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/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 = { diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index c102f2e5b7..f6b411ac65 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/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 diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index bc2383bd9b..893edfe2fb 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/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(); } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5150a81e5a..64bd2c44b7 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/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(lroundf(outputs[i])); if (output == DSHOT_DISARM_VALUE) { diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index bb67996acd..fc2909e20d 100644 --- a/src/drivers/dshot/DShot.h +++ b/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: diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index f5235f904a..c8fa59e637 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/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(lroundf(outputs[i])); + } + + _pwm_out->send_output_pwm(hw_outputs, num_outputs); return true; } diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp index ab8b41653e..d63e29c607 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/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; diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index b83bea3923..5b290bfa5b 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/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(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); } } diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index bb32fb7152..49c0b35032 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/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(lroundf(outputs[i]))); } } } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index bae339b511..f055f1056d 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/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; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6e61f6e736..671651ed60 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/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(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(lroundf(_mixing_output.actualFailsafeValue(i))); } io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators); diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index bf1b608e91..ccadd119a7 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/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(lroundf(fabs(data) * INT8_MAX)); sendTransaction(command, &byte, 1); } diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index fd7050fe7a..d0ab6f75fa 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/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); diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index 1a4b58691c..6934cd28f4 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/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 diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 0c41c7fca5..2e3fa6ed9c 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/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(lroundf(outputs[2])); + motor_out[1] = static_cast(lroundf(outputs[1])); + motor_out[2] = static_cast(lroundf(outputs[3])); + motor_out[3] = static_cast(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(lroundf(outputs[3])); + motor_out[1] = static_cast(lroundf(outputs[0])); + motor_out[2] = static_cast(lroundf(outputs[4])); + motor_out[3] = static_cast(lroundf(outputs[2])); + motor_out[4] = static_cast(lroundf(outputs[1])); + motor_out[5] = static_cast(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(lroundf(outputs[i])); } break; diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp index 1b9ee392e3..2593368aac 100644 --- a/src/drivers/tap_esc/TAP_ESC.hpp +++ b/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: diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index d44b565131..b19468ff54 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/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(outputs[i])); + msg.cmd.push_back(static_cast(lroundf(outputs[i]))); } _uavcan_pub_raw_cmd.broadcast(msg); diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 9a53820bff..8adcafe1b1 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/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 diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp index 1af9a7173c..85b4b92d29 100644 --- a/src/drivers/uavcan/actuators/servo.cpp +++ b/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); } diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp index 78d6f233e1..9c464d9cc4 100644 --- a/src/drivers/uavcan/actuators/servo.hpp +++ b/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; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 37b6f0cce9..4cd8ade8f0 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 56f7c445fe..4f84ff783e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/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; } diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index a6efe87eb1..e6e24b0b08 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/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(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); diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index 6b417c17db..8b6f916287 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/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(); diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index eb67188018..e47fed82a7 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -144,9 +144,9 @@ void MixingOutput::printStatus() const PX4_INFO_RAW("Channel Configuration:\n"); for (unsigned i = 0; i < _max_num_outputs; i++) { - PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d, center: %d\n", i, - (int)_function_assignment[i], _current_output_value[i], - actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i], _center_value[i]); + PX4_INFO_RAW("Channel %2d: func: %3d, value: %.2f, failsafe: %.2f, disarmed: %d, min: %d, max: %d, center: %d\n", + i, (int)_function_assignment[i], (double)_current_output_value[i], (double)actualFailsafeValue(i), + _disarmed_value[i], _min_value[i], _max_value[i], _center_value[i]); } } @@ -509,15 +509,15 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat // Doing so makes calibrations consistent among different configurations and hence PWM minimum and maximum have a consistent effect // hence the defaults for these parameters also make most setups work out of the box if (_armed.in_esc_calibration_mode) { - static constexpr uint16_t PWM_CALIBRATION_LOW = 1000; - static constexpr uint16_t PWM_CALIBRATION_HIGH = 2000; + static constexpr float PWM_CALIBRATION_LOW = 1000.f; + static constexpr float PWM_CALIBRATION_HIGH = 2000.f; for (int i = 0; i < _max_num_outputs; i++) { - if (_current_output_value[i] == _min_value[i]) { + if (fabsf(_current_output_value[i] - (float)_min_value[i]) < 0.5f) { _current_output_value[i] = PWM_CALIBRATION_LOW; } - if (_current_output_value[i] == _max_value[i]) { + if (fabsf(_current_output_value[i] - (float)_max_value[i]) < 0.5f) { _current_output_value[i] = PWM_CALIBRATION_HIGH; } } @@ -532,7 +532,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat } } -uint16_t MixingOutput::output_limit_calc_single(int i, float value) const +float MixingOutput::output_limit_calc_single(int i, float value) const { // check for invalid / disabled channels if (!PX4_ISFINITE(value)) { @@ -552,27 +552,15 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const && _param_handles[i].center != PARAM_INVALID && _center_value[i] >= 800 && _center_value[i] <= 2200) { - - /* bi-linear interpolation */ - if (value < 0.0f) { - output = math::interpolate(value, -1.f, 0.0f, - static_cast(_min_value[i]), static_cast(_center_value[i])); - - } else { - output = math::interpolate(value, 0.0f, 1.0f, - static_cast(_center_value[i]), static_cast(_max_value[i])); - } - + output = math::interpolateNXY(value, {-1.f, 0.f, 1.f}, {(float)_min_value[i], (float)_center_value[i], (float)_max_value[i]}); } // Everything except servos, or if center is not set else { - output = math::interpolate(value, -1.f, 1.f, - static_cast(_min_value[i]), static_cast(_max_value[i])); + output = math::interpolate(value, -1.f, 1.f, static_cast(_min_value[i]), static_cast(_max_value[i])); } - return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); - + return output; } void @@ -647,7 +635,7 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const for (int i = 0; i < num_channels; i++) { // Ramp from disarmed value to currently desired output that would apply without ramp - uint16_t desired_output = output_limit_calc_single(i, output[i]); + float desired_output = output_limit_calc_single(i, output[i]); _current_output_value[i] = _disarmed_value[i] + progress * (desired_output - _disarmed_value[i]); } } @@ -688,10 +676,9 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output } } -uint16_t -MixingOutput::actualFailsafeValue(int index) const +float MixingOutput::actualFailsafeValue(int index) const { - uint16_t value = 0; + float value = 0; if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function float default_failsafe = NAN; @@ -703,7 +690,7 @@ MixingOutput::actualFailsafeValue(int index) const value = output_limit_calc_single(index, default_failsafe); } else { - value = _failsafe_value[index]; + value = static_cast(_failsafe_value[index]); } return value; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3e4cec0d51..08e1a58e1c 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -84,8 +84,7 @@ public: * @param num_control_groups_updated number of actuator_control groups updated * @return if true, the update got handled, and actuator_outputs can be published */ - virtual bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) = 0; + virtual bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; /** called whenever the mixer gets updated/reset */ virtual void mixerChanged() {} @@ -186,7 +185,7 @@ public: /** * Returns the actual failsafe value taking into account the assigned function */ - uint16_t actualFailsafeValue(int index) const; + float actualFailsafeValue(int index) const; void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; } @@ -207,7 +206,7 @@ public: protected: void updateParams() override; - uint16_t output_limit_calc_single(int i, float value) const; + float output_limit_calc_single(int i, float value) const; private: @@ -247,7 +246,7 @@ private: uint16_t _min_value[MAX_ACTUATORS] {}; uint16_t _center_value[MAX_ACTUATORS] {}; uint16_t _max_value[MAX_ACTUATORS] {}; - uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) + float _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction enum class OutputLimitState { diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index 7f3a6feede..5a6f08ed86 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -87,8 +87,7 @@ public: was_scheduled = true; } - 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 { memcpy(outputs, outputs_, sizeof(outputs)); num_outputs = num_outputs_; @@ -168,7 +167,7 @@ public: mixer_changed = false; } - uint16_t outputs[MAX_ACTUATORS] {}; + float outputs[MAX_ACTUATORS] {}; int num_outputs{0}; int num_updates{0}; bool was_scheduled{false}; @@ -480,7 +479,7 @@ public: bool support_esc_calibration, bool ramp_up = true) : MixingOutput(param_prefix, max_num_outputs, interface, scheduling_policy, support_esc_calibration, ramp_up) {}; - uint16_t output_limit_calc_single(int i, float value) const { return MixingOutput::output_limit_calc_single(i, value); } + float output_limit_calc_single(int i, float value) const { return MixingOutput::output_limit_calc_single(i, value); } }; TEST_F(MixerModuleTest, OutputLimitCalcSingle) @@ -501,8 +500,10 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 2000); EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 1000); // Way ouf of range EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 2000); - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0005), 1500); // Rounding down - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0015), 1501); // Rounding up + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0005), 1500.25f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.0005))), 1500); // Rounding down + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0015), 1500.75f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.0015))), 1501); // Rounding up EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.002), 1501); // Exact value mixing_output.setAllMinValues(0); // lower range [0,20] @@ -517,8 +518,10 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 20); EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 0); // Way ouf of range EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 20); - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 10); // Rounding down - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 11); // Rounding up + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 10.25f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.025))), 10); // Rounding down + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 10.75f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.075))), 11); // Rounding up EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.1), 11); // Exact value mixing_output.setAllMinValues(20); // inverted range [20,0] @@ -533,7 +536,9 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 0); EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 20); // Way ouf of range EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 0); - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 10); // Rounding down - EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 9); // Rounding up + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 9.75f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.025))), 10); // Rounding down + EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 9.25f); + EXPECT_EQ(static_cast(lroundf(mixing_output.output_limit_calc_single(0, 0.075))), 9); // Rounding up EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.1), 9); // Exact value } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index f989434068..a4e9e55686 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/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; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index 5ffe105b5d..c55c743f85 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/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; } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 38c1c1dfde..b90ec9789e 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/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); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 0e6759408b..6182de8cea 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/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; } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 79a57c7ef3..8cef805c53 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/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; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index 7685bd14a7..7b40889488 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/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; } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index a789c45ced..9fb09ba4bd 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/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); diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index 693ba9be1a..706d11c366 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/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;