diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b2593656..5df12b4ea6 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -302,12 +302,6 @@ When set to -1 (default), the value depends on the function (see {:}). for key, label, param_suffix, description in standard_params_array: if key in standard_params: - # values must be in range of an uint16_t - if standard_params[key]['min'] < 0: - raise Exception('minimum value for {:} expected >= 0 (got {:})'.format(key, standard_params[key]['min'])) - if standard_params[key]['max'] >= 1<<16: - raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max'])) - if key == 'failsafe': standard_params[key]['default'] = -1 standard_params[key]['min'] = -1 @@ -317,7 +311,7 @@ When set to -1 (default), the value depends on the function (see {:}). 'short': channel_label+' ${i} '+label+' Value', 'long': description }, - 'type': 'int32', + 'type': 'float', 'instance_start': instance_start, 'instance_start_label': instance_start_label, 'num_instances': num_channels, diff --git a/msg/ActuatorOutputs.msg b/msg/ActuatorOutputs.msg index 3209e54e34..0406c998cc 100644 --- a/msg/ActuatorOutputs.msg +++ b/msg/ActuatorOutputs.msg @@ -1,7 +1,7 @@ uint64 timestamp # time since system start (microseconds) -uint8 NUM_ACTUATOR_OUTPUTS = 16 -uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking -uint32 noutputs # valid outputs + +uint8 NUM_ACTUATOR_OUTPUTS = 16 +uint8 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]) diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/modal_io/modal_io.cpp index 1e5a2bc08c..f899a248d3 100644 --- a/src/drivers/actuators/modal_io/modal_io.cpp +++ b/src/drivers/actuators/modal_io/modal_io.cpp @@ -877,7 +877,7 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control) } } -void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) +void ModalIo::mix_turtle_mode(float outputs[MAX_ACTUATORS]) { bool use_pitch = true; bool use_roll = true; @@ -1052,8 +1052,7 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) } /* OutputModuleInterface */ -bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool ModalIo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) { return false; diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/modal_io/modal_io.hpp index 196728aab7..250b59c0f4 100644 --- a/src/drivers/actuators/modal_io/modal_io.hpp +++ b/src/drivers/actuators/modal_io/modal_io.hpp @@ -75,8 +75,7 @@ public: int print_status() override; /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; virtual int init(); @@ -225,6 +224,6 @@ private: int parse_response(uint8_t *buf, uint8_t len, bool print_feedback); int flush_uart_rx(); int check_for_esc_timeout(); - void mix_turtle_mode(uint16_t outputs[]); + void mix_turtle_mode(float outputs[]); void handle_actuator_test(); }; diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 8d3f75fba6..de0053664a 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -97,7 +97,7 @@ public: } }; - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) + void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { if (_port_id > 0) { reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; @@ -105,7 +105,7 @@ public: for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { if (i < num_outputs) { - msg_sp.value[i] = static_cast(outputs[i]); + msg_sp.value[i] = outputs[i]; } else { // "unset" values published as NaN diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 73de1992ff..562295795f 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -392,8 +392,7 @@ void CyphalNode::sendHeartbeat() } } -bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool UavcanMixingInterface::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { // 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 004a509f40..b380d12f66 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(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; void printInfo() { _mixing_output.printStatus(); } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efb..f5eaf711d7 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -336,8 +336,7 @@ void DShot::mixerChanged() update_telemetry_num_motors(); } -bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool DShot::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { if (!_outputs_on) { return false; @@ -388,7 +387,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], for (int i = 0; i < (int)num_outputs; i++) { - uint16_t output = outputs[i]; + uint16_t output = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX); if (output == DSHOT_DISARM_VALUE) { up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb7..4fde64903b 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -93,8 +93,7 @@ public: bool telemetry_enabled() const { return _telemetry != nullptr; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) 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 7712973051..2199a767f2 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -95,10 +95,16 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool LinuxPWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { - _pwm_out->send_output_pwm(outputs, num_outputs); + uint16_t out[MAX_ACTUATORS] {}; + + for (unsigned i = 0; i < num_outputs; ++i) { + out[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX); + } + + _pwm_out->send_output_pwm(out, 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 ca3ab3829e..65e4da25f8 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -71,8 +71,7 @@ public: int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; private: static constexpr int MAX_ACTUATORS = 8; diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index 7eccd6c04b..f60fed677d 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -34,8 +34,10 @@ #include #include #include + #include "PCA9685.h" +#include #ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL #define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ @@ -95,15 +97,18 @@ int PCA9685::init() return PX4_OK; } -int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) +int PCA9685::updatePWM(const float outputs[PCA9685_PWM_CHANNEL_COUNT], unsigned num_outputs) { if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) { num_outputs = PCA9685_PWM_CHANNEL_COUNT; PX4_DEBUG("PCA9685 can only drive up to 16 channels"); } - uint16_t out[PCA9685_PWM_CHANNEL_COUNT]; - memcpy(out, outputs, sizeof(uint16_t) * num_outputs); + uint16_t out[PCA9685_PWM_CHANNEL_COUNT] {}; + + for (unsigned i = 0; i < num_outputs; ++i) { + out[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX); + } for (unsigned i = 0; i < num_outputs; ++i) { out[i] = calcRawFromPulse(out[i]); diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index e8bd5eb4d8..f1ca2956e3 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -100,7 +100,7 @@ public: * * *output: pulse width, us */ - int updatePWM(const uint16_t *outputs, unsigned num_outputs); + int updatePWM(const float outputs[PCA9685_PWM_CHANNEL_COUNT], unsigned num_outputs); /* * Set PWM frequency to new value. diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 0e9bfa63df..ec1bbb5730 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -70,8 +70,7 @@ public: static int custom_command(int argc, char *argv[]); static int print_usage(const char *reason = nullptr); - bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, - unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; int print_status() override; @@ -136,12 +135,13 @@ int PCA9685Wrapper::init() return PX4_OK; } -bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, - unsigned num_control_groups_updated) +bool PCA9685Wrapper::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { - if (state != STATE::RUNNING) { return false; } + if (state != STATE::RUNNING) { + return false; + } - uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {}; + uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] {}; num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs; for (uint8_t i = 0; i < num_outputs; ++i) { @@ -149,7 +149,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned low_level_outputs[i] = outputs[i]; } else { - low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); + uint16_t pulse_width = outputs[i]; + low_level_outputs[i] = pca9685->calcRawFromPulse(pulse_width); } } diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 6b482a36f7..5fab267d08 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -125,8 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on) return true; } -bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool PWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { /* output to the servos */ if (_pwm_initialized) { @@ -137,17 +136,13 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } if (_pwm_mask & (1 << i)) { - up_pwm_servo_set(i, outputs[i]); + uint16_t output = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX); + up_pwm_servo_set(i, output); } } } - /* Trigger all timer's channels in Oneshot mode to fire - * the oneshots with updated values. - */ - if (num_control_groups_updated > 0) { - up_pwm_update(_pwm_mask); - } + up_pwm_update(_pwm_mask); return true; } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 55d498a8fc..4cb3f1443c 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -71,8 +71,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; private: void Run() override; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 12ab205c69..66935d677e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -153,8 +153,7 @@ public: uint16_t system_status() const { return _status; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; private: void Run() override; @@ -361,12 +360,17 @@ PX4IO::~PX4IO() perf_free(_interface_write_perf); } -bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool PX4IO::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { if (!_test_fmu_fail) { /* output to the servos */ - io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); + uint16_t outputs_uint16[MAX_ACTUATORS] {}; + + for (unsigned i = 0; i < num_outputs; i++) { + outputs_uint16[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX); + } + + io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs_uint16, num_outputs); } return true; diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index d4aeeee5d3..8f9599b947 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -154,11 +154,10 @@ int Roboclaw::initializeUART() } } -bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) +bool Roboclaw::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { - float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; - float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f; + float right_motor_output = (outputs[0] - 128.0f) / 127.f; + float left_motor_output = (outputs[1] - 128.0f) / 127.f; if (stop_motors) { setMotorSpeed(Motor::Right, 0.f); diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index c58ef1bf5e..133f1a3e23 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -77,8 +77,7 @@ public: void Run() override; /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; void setMotorSpeed(Motor motor, float value); ///< rev/sec void setMotorDutyCycle(Motor motor, float value); diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 0d37dda2c0..b94446d7d9 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -241,8 +241,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet) tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1); } -bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool TAP_ESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { if (_initialized) { uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM] {}; diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp index 673237a1ca..52728313ba 100644 --- a/src/drivers/tap_esc/TAP_ESC.hpp +++ b/src/drivers/tap_esc/TAP_ESC.hpp @@ -99,8 +99,7 @@ public: int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; private: diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 12f5d8600b..99ce4661e7 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -70,7 +70,7 @@ UavcanEscController::init() } void -UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) +UavcanEscController::update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { /* * Rate limiting - we don't want to congest the bus @@ -90,7 +90,7 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA uavcan::equipment::esc::RawCommand msg; for (unsigned i = 0; i < num_outputs; i++) { - if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) { + if (stop_motors || !PX4_ISFINITE(outputs[i]) || (outputs[i] >= DISARMED_OUTPUT_VALUE)) { msg.cmd.push_back(static_cast(0)); } else { diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 516b147214..3c68671fa8 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -69,7 +69,7 @@ public: int init(); - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); + void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs); /** * 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 56fdea4bba..3d08358a2a 100644 --- a/src/drivers/uavcan/actuators/servo.cpp +++ b/src/drivers/uavcan/actuators/servo.cpp @@ -45,7 +45,7 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) : } void -UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) +UavcanServoController::update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { uavcan::equipment::actuator::ArrayCommand msg; @@ -53,7 +53,7 @@ UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACT 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]; msg.commands.push_back(cmd); } diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp index f0f1e97fb6..8afee58912 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(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); + void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs); private: uavcan::INode &_node; diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml index 9c2ba643dc..3d889b7244 100644 --- a/src/drivers/uavcan/module.yaml +++ b/src/drivers/uavcan/module.yaml @@ -12,16 +12,16 @@ actuator_output: group_label: 'ESCs' channel_label: 'ESC' standard_params: - min: { min: 0, max: 8191, default: 1 } - max: { min: 0, max: 8191, default: 8191 } + min: { min: -8192, max: 8191, default: 1 } + max: { min: -8192, max: 8191, default: 8191 } failsafe: { min: 0, max: 8191 } num_channels: 8 - param_prefix: UAVCAN_SV group_label: 'Servos' channel_label: 'Servo' standard_params: - disarmed: { min: 0, max: 1000, default: 500 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 1000, default: 1000 } - failsafe: { min: 0, max: 1000 } + disarmed: { min: -1, max: 1, default: 0 } + min: { min: -1, max: 1, default: -1 } + max: { min: -1, max: 1, default: 1 } + failsafe: { min: -1, max: 1 } num_channels: 8 diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 277b2526dd..9bd2047da2 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -975,8 +975,7 @@ UavcanNode::Run() } #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) -bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { _esc_controller.update_outputs(stop_motors, outputs, num_outputs); return true; @@ -1005,8 +1004,7 @@ void UavcanMixingInterfaceESC::mixerChanged() _esc_controller.set_rotor_count(rotor_count); } -bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { _servo_controller.update_outputs(stop_motors, outputs, num_outputs); return true; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 54119259ef..1a5e88108e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -122,8 +122,7 @@ public: _node_mutex(node_mutex), _esc_controller(esc_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; void mixerChanged() override; @@ -153,8 +152,7 @@ public: _node_mutex(node_mutex), _servo_controller(servo_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 8aea8c8cc6..779f7c6b45 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -90,7 +90,7 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou initParamHandles(); for (unsigned i = 0; i < MAX_ACTUATORS; i++) { - _failsafe_value[i] = UINT16_MAX; + _failsafe_value[i] = NAN; } updateParams(); @@ -141,9 +141,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\n", i, - (int)_function_assignment[i], _current_output_value[i], - actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]); + PX4_INFO_RAW("Channel %i: func: %3i, value: %.1f, failsafe: %.1f, disarmed: %.1f, min: %.1f, max: %.1f\n", i, + (int)_function_assignment[i], (double)_current_output_value[i], + (double)actualFailsafeValue(i), (double)_disarmed_value[i], (double)_min_value[i], (double)_max_value[i]); } } @@ -164,26 +164,28 @@ void MixingOutput::updateParams() // we set _function_assignment[i] later to ensure _functions[i] is updated at the same time } - if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) { - _disarmed_value[i] = val; + float fval; + + if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &fval) == 0) { + _disarmed_value[i] = fval; } - if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) { - _min_value[i] = val; + if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &fval) == 0) { + _min_value[i] = fval; } - if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) { - _max_value[i] = val; + if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &fval) == 0) { + _max_value[i] = fval; } if (_min_value[i] > _max_value[i]) { - uint16_t tmp = _min_value[i]; + float tmp = _min_value[i]; _min_value[i] = _max_value[i]; _max_value[i] = tmp; } - if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { - _failsafe_value[i] = val; + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &fval) == 0) { + _failsafe_value[i] = fval; } } @@ -363,7 +365,7 @@ void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us) } } -void MixingOutput::setAllMinValues(uint16_t value) +void MixingOutput::setAllMinValues(float value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _param_handles[i].min = PARAM_INVALID; @@ -371,7 +373,7 @@ void MixingOutput::setAllMinValues(uint16_t value) } } -void MixingOutput::setAllMaxValues(uint16_t value) +void MixingOutput::setAllMaxValues(float value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _param_handles[i].max = PARAM_INVALID; @@ -379,7 +381,7 @@ void MixingOutput::setAllMaxValues(uint16_t value) } } -void MixingOutput::setAllFailsafeValues(uint16_t value) +void MixingOutput::setAllFailsafeValues(float value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _param_handles[i].failsafe = PARAM_INVALID; @@ -387,7 +389,7 @@ void MixingOutput::setAllFailsafeValues(uint16_t value) } } -void MixingOutput::setAllDisarmedValues(uint16_t value) +void MixingOutput::setAllDisarmedValues(float value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _param_handles[i].disarmed = PARAM_INVALID; @@ -417,9 +419,6 @@ bool MixingOutput::update() _throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode; } - // only used for sitl with lockstep - bool has_updates = _subscription_callback && _subscription_callback->updated(); - // update topics for (int i = 0; i < MAX_ACTUATORS && _function_allocated[i]; ++i) { _function_allocated[i]->update(); @@ -460,14 +459,14 @@ bool MixingOutput::update() _actuator_test.overrideValues(outputs, _max_num_outputs); } - limitAndUpdateOutputs(outputs, has_updates); + limitAndUpdateOutputs(outputs); } return true; } void -MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates) +MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS]) { bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode(); @@ -498,18 +497,18 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat static constexpr uint16_t PWM_CALIBRATION_HIGH = 2000; for (int i = 0; i < _max_num_outputs; i++) { - if (_current_output_value[i] == _min_value[i]) { + if (static_cast(_current_output_value[i]) == static_cast(_min_value[i])) { _current_output_value[i] = PWM_CALIBRATION_LOW; } - if (_current_output_value[i] == _max_value[i]) { + if (static_cast(_current_output_value[i]) == static_cast(_max_value[i])) { _current_output_value[i] = PWM_CALIBRATION_HIGH; } } } /* now return the outputs to the driver */ - if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) { + if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs)) { actuator_outputs_s actuator_outputs{}; setAndPublishActuatorOutputs(_max_num_outputs, actuator_outputs); @@ -517,7 +516,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)) { @@ -528,7 +527,7 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const value = -1.f * value; } - uint16_t effective_output = value * (_max_value[i] - _min_value[i]) / 2 + (_max_value[i] + _min_value[i]) / 2; + float effective_output = value * (_max_value[i] - _min_value[i]) / 2.f + (_max_value[i] + _min_value[i]) / 2.f; // last line of defense against invalid inputs return math::constrain(effective_output, _min_value[i], _max_value[i]); @@ -599,12 +598,8 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const break; case OutputLimitState::RAMP: { - hrt_abstime diff = hrt_elapsed_time(&_output_time_armed); - float progress = static_cast(diff) / RAMP_TIME_US; - - if (progress > 1.f) { - progress = 1.f; - } + float elapsed_us = static_cast(hrt_elapsed_time(&_output_time_armed)); + float progress = math::constrain(elapsed_us / RAMP_TIME_US, 0.f, 1.f); for (int i = 0; i < num_channels; i++) { // Ramp from disarmed value to currently desired output that would apply without ramp @@ -649,19 +644,18 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output } } -uint16_t +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; + if (!PX4_ISFINITE(_failsafe_value[index])) { // if set to default, use the one provided by the function if (_functions[index]) { - default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]); - } + float default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]); - value = output_limit_calc_single(index, default_failsafe); + value = output_limit_calc_single(index, default_failsafe); + } } else { value = _failsafe_value[index]; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 0d8a495271..e6120273f8 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -82,11 +82,9 @@ public: * might still be stopped via outputs[i] == disarmed_value) * @param outputs individual actuator outputs in range [min, max] or failsafe/disarmed value * @param num_outputs number of outputs (<= max_num_outputs) - * @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(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) = 0; + virtual bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) = 0; /** called whenever the mixer gets updated/reset */ virtual void mixerChanged() {} @@ -165,17 +163,18 @@ public: const actuator_armed_s &armed() const { return _armed; } - void setAllFailsafeValues(uint16_t value); - void setAllDisarmedValues(uint16_t value); - void setAllMinValues(uint16_t value); - void setAllMaxValues(uint16_t value); + void setAllFailsafeValues(float value); + void setAllDisarmedValues(float value); + void setAllMinValues(float value); + void setAllMaxValues(float value); uint16_t &reverseOutputMask() { return _reverse_output_mask; } - uint16_t &failsafeValue(int index) { return _failsafe_value[index]; } + + float &failsafeValue(int index) { return _failsafe_value[index]; } /** Disarmed values: disarmedValue < minValue needs to hold */ - uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } - uint16_t &minValue(int index) { return _min_value[index]; } - uint16_t &maxValue(int index) { return _max_value[index]; } + float &disarmedValue(int index) { return _disarmed_value[index]; } + float &minValue(int index) { return _min_value[index]; } + float &maxValue(int index) { return _max_value[index]; } param_t functionParamHandle(int index) const { return _param_handles[index].function; } param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; } @@ -185,7 +184,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; } @@ -222,9 +221,9 @@ private: void initParamHandles(); - void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates); + void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS]); - uint16_t output_limit_calc_single(int i, float value) const; + inline float output_limit_calc_single(int i, float value) const; void output_limit_calc(const bool armed, const int num_channels, const float outputs[MAX_ACTUATORS]); @@ -241,11 +240,12 @@ private: px4_sem_t _lock; /**< lock to protect access to work queue changes (includes ScheduleNow calls from another thread) */ - uint16_t _failsafe_value[MAX_ACTUATORS] {}; - uint16_t _disarmed_value[MAX_ACTUATORS] {}; - uint16_t _min_value[MAX_ACTUATORS] {}; - uint16_t _max_value[MAX_ACTUATORS] {}; - uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) + float _failsafe_value[MAX_ACTUATORS] {}; + float _disarmed_value[MAX_ACTUATORS] {}; + float _min_value[MAX_ACTUATORS] {}; + float _max_value[MAX_ACTUATORS] {}; + 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 fe6cb79772..0f0ccda5f3 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(bool stop_motors, uint16_t outputs_[MAX_ACTUATORS], - unsigned num_outputs_, unsigned num_control_groups_updated) override + bool updateOutputs(bool stop_motors, float outputs_[MAX_ACTUATORS], unsigned num_outputs_) 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}; @@ -454,10 +453,10 @@ TEST_F(MixerModuleTest, prearm) for (int i = 0; i < max_num_outputs; ++i) { if (i == 1) { - EXPECT_EQ(test_module.outputs[i], max_value); + EXPECT_FLOAT_EQ(test_module.outputs[i], max_value); } else { - EXPECT_EQ(test_module.outputs[i], disarmed_value); + EXPECT_FLOAT_EQ(test_module.outputs[i], disarmed_value); } } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index c1a88f7bda..2a6e70206c 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -60,8 +60,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) return true; } -bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { unsigned active_output_count = 0; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index 316ff6195a..85ab76cd0d 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -56,8 +56,7 @@ public: _node_mutex(node_mutex) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) 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 9c7969b2dc..2b7baa20c9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -53,22 +53,20 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) return true; } -bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { bool updated = false; - // cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1] int i = 0; for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { - gz::msgs::Double servo_output; + gz::msgs::Double servo_output{}; ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; - servo_output.set_data(output); + servo_output.set_data(outputs[i]); if (servo_pub.Valid()) { servo_pub.Publish(servo_output); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 63345b3aed..cca0f22961 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -49,8 +49,7 @@ public: _node_mutex(node_mutex) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index d4258bd0e9..9077084cd0 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -11,17 +11,17 @@ actuator_output: group_label: 'ESCs' channel_label: 'ESC' standard_params: - disarmed: { min: 0, max: 1000, default: 0 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 1000, default: 1000 } - failsafe: { min: 0, max: 1000 } + disarmed: { min: -8192, max: 8191, default: 0 } + min: { min: -8192, max: 8191, default: 0 } + max: { min: -8192, max: 8191, default: 8191 } + failsafe: { min: -8192, max: 8191 } num_channels: 8 - param_prefix: SIM_GZ_SV group_label: 'Servos' channel_label: 'Servo' standard_params: - disarmed: { min: 0, max: 1000, default: 500 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 1000, default: 1000 } - failsafe: { min: 0, max: 1000 } + disarmed: { min: -3.14159, max: 3.14159, default: 0 } + min: { min: -3.14159, max: 3.14159, default: -3.14159 } + max: { min: -3.14159, max: 3.14159, default: 3.14159 } + failsafe: { min: -3.14159, max: 3.14159 } num_channels: 8 diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365..efe10ab9c0 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -58,43 +58,37 @@ PWMSim::~PWMSim() perf_free(_interval_perf); } -bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) +bool PWMSim::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) { - // 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; + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = num_outputs; - const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); + const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); - for (int i = 0; i < (int)num_outputs; i++) { - if (outputs[i] != PWM_SIM_DISARMED_MAGIC) { + 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]; + 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); + 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; - } + } 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; + actuator_outputs.timestamp = hrt_absolute_time(); + _actuator_outputs_sim_pub.publish(actuator_outputs); + return true; } void PWMSim::Run() diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e..7e0fe8d933 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -70,8 +70,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override; private: void Run() override;