diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b25936568e..5df12b4ea6a3 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -302,12 +302,6 @@ def add_local_param(param_name, param_def): 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 @@ def add_local_param(param_name, param_def): '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/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/modal_io/modal_io.cpp index 1e5a2bc08c43..1664a078a889 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,7 +1052,7 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) } /* OutputModuleInterface */ -bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool ModalIo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) { diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/modal_io/modal_io.hpp index 196728aab7f1..f830b355cb7b 100644 --- a/src/drivers/actuators/modal_io/modal_io.hpp +++ b/src/drivers/actuators/modal_io/modal_io.hpp @@ -75,7 +75,7 @@ class ModalIo : public ModuleBase, public OutputModuleInterface int print_status() override; /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; virtual int init(); @@ -225,6 +225,6 @@ class ModalIo : public ModuleBase, public OutputModuleInterface 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 8d3f75fba632..de0053664a1f 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -97,7 +97,7 @@ class UavcanEscController : public UavcanPublisher } }; - 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 @@ class UavcanEscController : public UavcanPublisher 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 73de1992ff51..542dd45ec9c1 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -392,7 +392,7 @@ void CyphalNode::sendHeartbeat() } } -bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool UavcanMixingInterface::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { // Note: This gets called from MixingOutput from within its update() function diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 004a509f4039..c41fb48af986 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -87,7 +87,7 @@ class UavcanMixingInterface : public OutputModuleInterface _node_mutex(node_mutex), _pub_manager(pub_manager) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, 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 5ae0150efba9..7fec9fcb4dea 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -336,7 +336,7 @@ void DShot::mixerChanged() update_telemetry_num_motors(); } -bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool DShot::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (!_outputs_on) { @@ -388,7 +388,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 4f314cebb75f..c42677122990 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -93,7 +93,7 @@ class DShot final : public ModuleBase, public OutputModuleInterface bool telemetry_enabled() const { return _telemetry != nullptr; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, 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 771297305126..bea609d5fb36 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -95,10 +95,17 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool LinuxPWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - _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 ca3ab3829ea1..4672f295f420 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -71,7 +71,7 @@ class LinuxPWMOut : public ModuleBase, public OutputModuleInterface int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index 7eccd6c04b3c..844fafabdc24 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -95,7 +95,7 @@ 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; @@ -103,7 +103,10 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) } uint16_t out[PCA9685_PWM_CHANNEL_COUNT]; - memcpy(out, outputs, sizeof(uint16_t) * num_outputs); + + for (unsigned i = 0; i < num_outputs; ++i) { + out[i] = (uint16_t)roundl((outputs[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution + } 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 e8bd5eb4d893..f1ca2956e350 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -100,7 +100,7 @@ class PCA9685 : public device::I2C * * *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 0e9bfa63dfcc..3c39363fac48 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -70,7 +70,7 @@ class PCA9685Wrapper : public ModuleBase, public OutputModuleInt 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, + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; int print_status() override; @@ -136,7 +136,7 @@ int PCA9685Wrapper::init() return PX4_OK; } -bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, +bool PCA9685Wrapper::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (state != STATE::RUNNING) { return false; } diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 6b482a36f77c..9ac19e21ae81 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -125,7 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on) return true; } -bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool PWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { /* output to the servos */ @@ -137,7 +137,8 @@ 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); } } } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 55d498a8fca1..3f7f747312f9 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -71,7 +71,7 @@ class PWMOut final : public ModuleBase, public OutputModuleInterface /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 12ab205c691e..22e204abf4e3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -153,7 +153,7 @@ class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleIn uint16_t system_status() const { return _status; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: @@ -361,12 +361,18 @@ PX4IO::~PX4IO() perf_free(_interface_write_perf); } -bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool PX4IO::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { 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 d4aeeee5d3ee..1f5136889afa 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -154,11 +154,11 @@ 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, + 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; + 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 c58ef1bf5eb6..ed228eb430ea 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -77,8 +77,8 @@ class Roboclaw : public ModuleBase, public OutputModuleInterface 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, + 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/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 0d37dda2c0a4..21d61b089e02 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -241,7 +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, +bool TAP_ESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (_initialized) { diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp index 673237a1cabb..e45fa04f8331 100644 --- a/src/drivers/tap_esc/TAP_ESC.hpp +++ b/src/drivers/tap_esc/TAP_ESC.hpp @@ -99,7 +99,7 @@ class TAP_ESC : public ModuleBase, public OutputModuleInterface int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, 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 12f5d8600bf2..99ce4661e7be 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 516b1472147c..3c68671fa88d 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -69,7 +69,7 @@ class UavcanEscController 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 56fdea4bbaff..3d08358a2adc 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 f0f1e97fb6a0..8afee5891274 100644 --- a/src/drivers/uavcan/actuators/servo.hpp +++ b/src/drivers/uavcan/actuators/servo.hpp @@ -52,7 +52,7 @@ class UavcanServoController 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 9c2ba643dce8..3d889b7244ad 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 277b2526ddd4..a740acb33b0b 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -975,7 +975,7 @@ UavcanNode::Run() } #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) -bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { _esc_controller.update_outputs(stop_motors, outputs, num_outputs); @@ -1005,7 +1005,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, +bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { _servo_controller.update_outputs(stop_motors, outputs, num_outputs); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 54119259ef16..4e025b6bba0e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -122,7 +122,7 @@ class UavcanMixingInterfaceESC : public OutputModuleInterface _node_mutex(node_mutex), _esc_controller(esc_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; void mixerChanged() override; @@ -153,7 +153,7 @@ class UavcanMixingInterfaceServo : public OutputModuleInterface _node_mutex(node_mutex), _servo_controller(servo_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) 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 8aea8c8cc6c3..b1d8decc7f4e 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; @@ -498,11 +500,11 @@ 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; } } @@ -517,7 +519,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 +530,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 +601,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 +647,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 0d8a4952710f..07704b694641 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -85,7 +85,7 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams * @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], + virtual bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; /** called whenever the mixer gets updated/reset */ @@ -165,17 +165,18 @@ class MixingOutput : public ModuleParams 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 +186,7 @@ class MixingOutput : public ModuleParams /** * 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; } @@ -224,7 +225,7 @@ class MixingOutput : public ModuleParams void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates); - 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 +242,12 @@ class MixingOutput : public ModuleParams 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 fe6cb7977293..0e67baf83116 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -87,7 +87,7 @@ class OutputModuleTest : public OutputModuleInterface was_scheduled = true; } - bool updateOutputs(bool stop_motors, uint16_t outputs_[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs_[MAX_ACTUATORS], unsigned num_outputs_, unsigned num_control_groups_updated) override { memcpy(outputs, outputs_, sizeof(outputs)); @@ -168,7 +168,7 @@ class OutputModuleTest : public OutputModuleInterface 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 +454,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 c1a88f7bda9c..095976cf4b13 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -60,7 +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, +bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, 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 316ff6195aab..5a48b0fcb00f 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -56,7 +56,7 @@ class GZMixingInterfaceESC : public OutputModuleInterface _node_mutex(node_mutex) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, 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 9c7969b2dc1e..cd31d87d6b11 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -53,22 +53,21 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) return true; } -bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, 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] 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 63345b3aed89..147ccef5c295 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -49,7 +49,7 @@ class GZMixingInterfaceServo : public OutputModuleInterface _node_mutex(node_mutex) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, 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/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index d4258bd0e99a..9077084cd025 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 c11a5eb365fe..b0ca1c9c0879 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -58,7 +58,7 @@ PWMSim::~PWMSim() perf_free(_interval_perf); } -bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool PWMSim::updateOutputs(bool stop_motors, 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) @@ -69,7 +69,7 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un 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 (outputs[i] > PWM_SIM_DISARMED_MAGIC) { 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 b16a9fac5e75..0e15735733c4 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -70,7 +70,7 @@ class PWMSim : public ModuleBase, public OutputModuleInterface /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: