dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP: output modules native units

This commit is contained in:
Daniel Agar 2023-01-31 10:09:18 -05:00
parent d259386987
commit 3d7f6ea09c
38 changed files with 172 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Tools/module_config/generate_params.py
View File

@ -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: for key, label, param_suffix, description in standard_params_array:
if key in standard_params: 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': if key == 'failsafe':
standard_params[key]['default'] = -1 standard_params[key]['default'] = -1
standard_params[key]['min'] = -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', 'short': channel_label+' ${i} '+label+' Value',
'long': description 'long': description
}, },
'type': 'int32', 'type': 'float',
'instance_start': instance_start, 'instance_start': instance_start,
'instance_start_label': instance_start_label, 'instance_start_label': instance_start_label,
'num_instances': num_channels, 'num_instances': num_channels,

4
msg/ActuatorOutputs.msg
View File

@ -1,7 +1,7 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
uint8 NUM_ACTUATOR_OUTPUTS = 16 uint8 NUM_ACTUATOR_OUTPUTS = 16
uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking uint8 noutputs # valid outputs
uint32 noutputs # valid outputs
float32[16] output # output data, in natural output units 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]) # actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])

5
src/drivers/actuators/modal_io/modal_io.cpp
View File

@ -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_pitch = true;
bool use_roll = true; bool use_roll = true;
@ -1052,8 +1052,7 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
} }
/* OutputModuleInterface */ /* 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_outputs, unsigned num_control_groups_updated)
{ {
if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) { if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) {
return false; return false;

5
src/drivers/actuators/modal_io/modal_io.hpp
View File

@ -75,8 +75,7 @@ public:
int print_status() override; int print_status() override;
/** @see OutputModuleInterface */ /** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
virtual int init(); virtual int init();
@ -225,6 +224,6 @@ private:
int parse_response(uint8_t *buf, uint8_t len, bool print_feedback); int parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
int flush_uart_rx(); int flush_uart_rx();
int check_for_esc_timeout(); int check_for_esc_timeout();
void mix_turtle_mode(uint16_t outputs[]); void mix_turtle_mode(float outputs[]);
void handle_actuator_test(); void handle_actuator_test();
}; };

4
src/drivers/cyphal/Actuators/EscClient.hpp
View File

@ -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) { if (_port_id > 0) {
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {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++) { for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
if (i < num_outputs) { if (i < num_outputs) {
msg_sp.value[i] = static_cast<float>(outputs[i]); msg_sp.value[i] = outputs[i];
} else { } else {
// "unset" values published as NaN // "unset" values published as NaN

3
src/drivers/cyphal/Cyphal.cpp
View File

@ -392,8 +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 // Note: This gets called from MixingOutput from within its update() function
// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect // Hence, the mutex lock in UavcanMixingInterface::Run() is in effect

3
src/drivers/cyphal/Cyphal.hpp
View File

@ -87,8 +87,7 @@ public:
_node_mutex(node_mutex), _node_mutex(node_mutex),
_pub_manager(pub_manager) {} _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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
void printInfo() { _mixing_output.printStatus(); } void printInfo() { _mixing_output.printStatus(); }

5
src/drivers/dshot/DShot.cpp
View File

@ -336,8 +336,7 @@ void DShot::mixerChanged()
update_telemetry_num_motors(); 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_outputs, unsigned num_control_groups_updated)
{ {
if (!_outputs_on) { if (!_outputs_on) {
return false; 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++) { 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) { if (output == DSHOT_DISARM_VALUE) {
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);

3
src/drivers/dshot/DShot.h
View File

@ -93,8 +93,7 @@ public:
bool telemetry_enabled() const { return _telemetry != nullptr; } 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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:

12
src/drivers/linux_pwm_out/linux_pwm_out.cpp
View File

@ -95,10 +95,16 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
return PX4_ERROR; 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_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; return true;
} }

3
src/drivers/linux_pwm_out/linux_pwm_out.hpp
View File

@ -71,8 +71,7 @@ public:
int init(); int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:
static constexpr int MAX_ACTUATORS = 8; static constexpr int MAX_ACTUATORS = 8;

11
src/drivers/pca9685_pwm_out/PCA9685.cpp
View File

@ -34,8 +34,10 @@
#include <px4_log.h> #include <px4_log.h>
#include <px4_defines.h> #include <px4_defines.h>
#include <cmath> #include <cmath>
#include "PCA9685.h" #include "PCA9685.h"
#include <lib/mathlib/mathlib.h>
#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL #ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL
#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ #define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ
@ -95,15 +97,18 @@ int PCA9685::init()
return PX4_OK; 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) { if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) {
num_outputs = PCA9685_PWM_CHANNEL_COUNT; num_outputs = PCA9685_PWM_CHANNEL_COUNT;
PX4_DEBUG("PCA9685 can only drive up to 16 channels"); PX4_DEBUG("PCA9685 can only drive up to 16 channels");
} }
uint16_t out[PCA9685_PWM_CHANNEL_COUNT]; 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] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
}
for (unsigned i = 0; i < num_outputs; ++i) { for (unsigned i = 0; i < num_outputs; ++i) {
out[i] = calcRawFromPulse(out[i]); out[i] = calcRawFromPulse(out[i]);

2
src/drivers/pca9685_pwm_out/PCA9685.h
View File

@ -100,7 +100,7 @@ public:
* *
* *output: pulse width, us * *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. * Set PWM frequency to new value.

15
src/drivers/pca9685_pwm_out/main.cpp
View File

@ -70,8 +70,7 @@ public:
static int custom_command(int argc, char *argv[]); static int custom_command(int argc, char *argv[]);
static int print_usage(const char *reason = nullptr); 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) override;
unsigned num_control_groups_updated) override;
int print_status() override; int print_status() override;
@ -136,12 +135,13 @@ int PCA9685Wrapper::init()
return PX4_OK; 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; } 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; num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs;
for (uint8_t i = 0; i < num_outputs; ++i) { 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]; low_level_outputs[i] = outputs[i];
} else { } else {
low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); uint16_t pulse_width = outputs[i];
low_level_outputs[i] = pca9685->calcRawFromPulse(pulse_width);
} }
} }

11
src/drivers/pwm_out/PWMOut.cpp
View File

@ -125,8 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on)
return true; 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_outputs, unsigned num_control_groups_updated)
{ {
/* output to the servos */ /* output to the servos */
if (_pwm_initialized) { if (_pwm_initialized) {
@ -137,17 +136,13 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
} }
if (_pwm_mask & (1 << i)) { 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; return true;
} }

3
src/drivers/pwm_out/PWMOut.hpp
View File

@ -71,8 +71,7 @@ public:
/** @see ModuleBase::print_status() */ /** @see ModuleBase::print_status() */
int print_status() override; 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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:
void Run() override; void Run() override;

14
src/drivers/px4io/px4io.cpp
View File

@ -153,8 +153,7 @@ public:
uint16_t system_status() const { return _status; } 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) override;
unsigned num_control_groups_updated) override;
private: private:
void Run() override; void Run() override;
@ -361,12 +360,17 @@ PX4IO::~PX4IO()
perf_free(_interface_write_perf); 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_outputs, unsigned num_control_groups_updated)
{ {
if (!_test_fmu_fail) { if (!_test_fmu_fail) {
/* output to the servos */ /* 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; return true;

7
src/drivers/roboclaw/Roboclaw.cpp
View File

@ -154,11 +154,10 @@ int Roboclaw::initializeUART()
} }
} }
bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool Roboclaw::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
unsigned num_outputs, unsigned num_control_groups_updated)
{ {
float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; float right_motor_output = (outputs[0] - 128.0f) / 127.f;
float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f; float left_motor_output = (outputs[1] - 128.0f) / 127.f;
if (stop_motors) { if (stop_motors) {
setMotorSpeed(Motor::Right, 0.f); setMotorSpeed(Motor::Right, 0.f);

3
src/drivers/roboclaw/Roboclaw.hpp
View File

@ -77,8 +77,7 @@ public:
void Run() override; void Run() override;
/** @see OutputModuleInterface */ /** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
void setMotorSpeed(Motor motor, float value); ///< rev/sec void setMotorSpeed(Motor motor, float value); ///< rev/sec
void setMotorDutyCycle(Motor motor, float value); void setMotorDutyCycle(Motor motor, float value);

3
src/drivers/tap_esc/TAP_ESC.cpp
View File

@ -241,8 +241,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet)
tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1); 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) { if (_initialized) {
uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM] {}; uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM] {};

3
src/drivers/tap_esc/TAP_ESC.hpp
View File

@ -99,8 +99,7 @@ public:
int init(); int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:

4
src/drivers/uavcan/actuators/esc.cpp
View File

@ -70,7 +70,7 @@ UavcanEscController::init()
} }
void 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 * 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; uavcan::equipment::esc::RawCommand msg;
for (unsigned i = 0; i < num_outputs; i++) { 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<unsigned>(0)); msg.cmd.push_back(static_cast<unsigned>(0));
} else { } else {

2
src/drivers/uavcan/actuators/esc.hpp
View File

@ -69,7 +69,7 @@ public:
int init(); 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 * Sets the number of rotors and enable timer

4
src/drivers/uavcan/actuators/servo.cpp
View File

@ -45,7 +45,7 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) :
} }
void 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; 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; uavcan::equipment::actuator::Command cmd;
cmd.actuator_id = i; cmd.actuator_id = i;
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS; 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); msg.commands.push_back(cmd);
} }

2
src/drivers/uavcan/actuators/servo.hpp
View File

@ -52,7 +52,7 @@ public:
UavcanServoController(uavcan::INode &node); UavcanServoController(uavcan::INode &node);
~UavcanServoController() = default; ~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: private:
uavcan::INode &_node; uavcan::INode &_node;

12
src/drivers/uavcan/module.yaml
View File

@ -12,16 +12,16 @@ actuator_output:
group_label: 'ESCs' group_label: 'ESCs'
channel_label: 'ESC' channel_label: 'ESC'
standard_params: standard_params:
min: { min: 0, max: 8191, default: 1 } min: { min: -8192, max: 8191, default: 1 }
max: { min: 0, max: 8191, default: 8191 } max: { min: -8192, max: 8191, default: 8191 }
failsafe: { min: 0, max: 8191 } failsafe: { min: 0, max: 8191 }
num_channels: 8 num_channels: 8
- param_prefix: UAVCAN_SV - param_prefix: UAVCAN_SV
group_label: 'Servos' group_label: 'Servos'
channel_label: 'Servo' channel_label: 'Servo'
standard_params: standard_params:
disarmed: { min: 0, max: 1000, default: 500 } disarmed: { min: -1, max: 1, default: 0 }
min: { min: 0, max: 1000, default: 0 } min: { min: -1, max: 1, default: -1 }
max: { min: 0, max: 1000, default: 1000 } max: { min: -1, max: 1, default: 1 }
failsafe: { min: 0, max: 1000 } failsafe: { min: -1, max: 1 }
num_channels: 8 num_channels: 8

6
src/drivers/uavcan/uavcan_main.cpp
View File

@ -975,8 +975,7 @@ UavcanNode::Run()
} }
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) #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); _esc_controller.update_outputs(stop_motors, outputs, num_outputs);
return true; return true;
@ -1005,8 +1004,7 @@ void UavcanMixingInterfaceESC::mixerChanged()
_esc_controller.set_rotor_count(rotor_count); _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); _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
return true; return true;

6
src/drivers/uavcan/uavcan_main.hpp
View File

@ -122,8 +122,7 @@ public:
_node_mutex(node_mutex), _node_mutex(node_mutex),
_esc_controller(esc_controller) {} _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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override; void mixerChanged() override;
@ -153,8 +152,7 @@ public:
_node_mutex(node_mutex), _node_mutex(node_mutex),
_servo_controller(servo_controller) {} _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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; } MixingOutput &mixingOutput() { return _mixing_output; }

72
src/lib/mixer_module/mixer_module.cpp
View File

@ -90,7 +90,7 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou
initParamHandles(); initParamHandles();
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_failsafe_value[i] = UINT16_MAX; _failsafe_value[i] = NAN;
} }
updateParams(); updateParams();
@ -141,9 +141,9 @@ void MixingOutput::printStatus() const
PX4_INFO_RAW("Channel Configuration:\n"); PX4_INFO_RAW("Channel Configuration:\n");
for (unsigned i = 0; i < _max_num_outputs; i++) { 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, PX4_INFO_RAW("Channel %i: func: %3i, value: %.1f, failsafe: %.1f, disarmed: %.1f, min: %.1f, max: %.1f\n", i,
(int)_function_assignment[i], _current_output_value[i], (int)_function_assignment[i], (double)_current_output_value[i],
actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_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 // 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) { float fval;
_disarmed_value[i] = val;
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) { if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &fval) == 0) {
_min_value[i] = val; _min_value[i] = fval;
} }
if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) { if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &fval) == 0) {
_max_value[i] = val; _max_value[i] = fval;
} }
if (_min_value[i] > _max_value[i]) { if (_min_value[i] > _max_value[i]) {
uint16_t tmp = _min_value[i]; float tmp = _min_value[i];
_min_value[i] = _max_value[i]; _min_value[i] = _max_value[i];
_max_value[i] = tmp; _max_value[i] = tmp;
} }
if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &fval) == 0) {
_failsafe_value[i] = val; _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++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_param_handles[i].min = PARAM_INVALID; _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++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_param_handles[i].max = PARAM_INVALID; _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++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_param_handles[i].failsafe = PARAM_INVALID; _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++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_param_handles[i].disarmed = PARAM_INVALID; _param_handles[i].disarmed = PARAM_INVALID;
@ -417,9 +419,6 @@ bool MixingOutput::update()
_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode; _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 // update topics
for (int i = 0; i < MAX_ACTUATORS && _function_allocated[i]; ++i) { for (int i = 0; i < MAX_ACTUATORS && _function_allocated[i]; ++i) {
_function_allocated[i]->update(); _function_allocated[i]->update();
@ -460,14 +459,14 @@ bool MixingOutput::update()
_actuator_test.overrideValues(outputs, _max_num_outputs); _actuator_test.overrideValues(outputs, _max_num_outputs);
} }
limitAndUpdateOutputs(outputs, has_updates); limitAndUpdateOutputs(outputs);
} }
return true; return true;
} }
void void
MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates) MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS])
{ {
bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode(); 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; static constexpr uint16_t PWM_CALIBRATION_HIGH = 2000;
for (int i = 0; i < _max_num_outputs; i++) { for (int i = 0; i < _max_num_outputs; i++) {
if (_current_output_value[i] == _min_value[i]) { if (static_cast<uint16_t>(_current_output_value[i]) == static_cast<uint16_t>(_min_value[i])) {
_current_output_value[i] = PWM_CALIBRATION_LOW; _current_output_value[i] = PWM_CALIBRATION_LOW;
} }
if (_current_output_value[i] == _max_value[i]) { if (static_cast<uint16_t>(_current_output_value[i]) == static_cast<uint16_t>(_max_value[i])) {
_current_output_value[i] = PWM_CALIBRATION_HIGH; _current_output_value[i] = PWM_CALIBRATION_HIGH;
} }
} }
} }
/* now return the outputs to the driver */ /* 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{}; actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(_max_num_outputs, 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 // check for invalid / disabled channels
if (!PX4_ISFINITE(value)) { if (!PX4_ISFINITE(value)) {
@ -528,7 +527,7 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const
value = -1.f * value; 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 // last line of defense against invalid inputs
return math::constrain(effective_output, _min_value[i], _max_value[i]); 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; break;
case OutputLimitState::RAMP: { case OutputLimitState::RAMP: {
hrt_abstime diff = hrt_elapsed_time(&_output_time_armed); float elapsed_us = static_cast<float>(hrt_elapsed_time(&_output_time_armed));
float progress = static_cast<float>(diff) / RAMP_TIME_US; float progress = math::constrain(elapsed_us / RAMP_TIME_US, 0.f, 1.f);
if (progress > 1.f) {
progress = 1.f;
}
for (int i = 0; i < num_channels; i++) { for (int i = 0; i < num_channels; i++) {
// Ramp from disarmed value to currently desired output that would apply without ramp // 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 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 if (!PX4_ISFINITE(_failsafe_value[index])) { // if set to default, use the one provided by the function
float default_failsafe = NAN;
if (_functions[index]) { 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 { } else {
value = _failsafe_value[index]; value = _failsafe_value[index];

38
src/lib/mixer_module/mixer_module.hpp
View File

@ -82,11 +82,9 @@ public:
* might still be stopped via outputs[i] == disarmed_value) * might still be stopped via outputs[i] == disarmed_value)
* @param outputs individual actuator outputs in range [min, max] or failsafe/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_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 * @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) = 0;
unsigned num_outputs, unsigned num_control_groups_updated) = 0;
/** called whenever the mixer gets updated/reset */ /** called whenever the mixer gets updated/reset */
virtual void mixerChanged() {} virtual void mixerChanged() {}
@ -165,17 +163,18 @@ public:
const actuator_armed_s &armed() const { return _armed; } const actuator_armed_s &armed() const { return _armed; }
void setAllFailsafeValues(uint16_t value); void setAllFailsafeValues(float value);
void setAllDisarmedValues(uint16_t value); void setAllDisarmedValues(float value);
void setAllMinValues(uint16_t value); void setAllMinValues(float value);
void setAllMaxValues(uint16_t value); void setAllMaxValues(float value);
uint16_t &reverseOutputMask() { return _reverse_output_mask; } 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 */ /** Disarmed values: disarmedValue < minValue needs to hold */
uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } float &disarmedValue(int index) { return _disarmed_value[index]; }
uint16_t &minValue(int index) { return _min_value[index]; } float &minValue(int index) { return _min_value[index]; }
uint16_t &maxValue(int index) { return _max_value[index]; } float &maxValue(int index) { return _max_value[index]; }
param_t functionParamHandle(int index) const { return _param_handles[index].function; } param_t functionParamHandle(int index) const { return _param_handles[index].function; }
param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; } 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 * 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; } void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; }
@ -222,9 +221,9 @@ private:
void initParamHandles(); 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]); 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) */ px4_sem_t _lock; /**< lock to protect access to work queue changes (includes ScheduleNow calls from another thread) */
uint16_t _failsafe_value[MAX_ACTUATORS] {}; float _failsafe_value[MAX_ACTUATORS] {};
uint16_t _disarmed_value[MAX_ACTUATORS] {}; float _disarmed_value[MAX_ACTUATORS] {};
uint16_t _min_value[MAX_ACTUATORS] {}; float _min_value[MAX_ACTUATORS] {};
uint16_t _max_value[MAX_ACTUATORS] {}; float _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 uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction
enum class OutputLimitState { enum class OutputLimitState {

9
src/lib/mixer_module/mixer_module_tests.cpp
View File

@ -87,8 +87,7 @@ public:
was_scheduled = true; 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_) override
unsigned num_outputs_, unsigned num_control_groups_updated) override
{ {
memcpy(outputs, outputs_, sizeof(outputs)); memcpy(outputs, outputs_, sizeof(outputs));
num_outputs = num_outputs_; num_outputs = num_outputs_;
@ -168,7 +167,7 @@ public:
mixer_changed = false; mixer_changed = false;
} }
uint16_t outputs[MAX_ACTUATORS] {}; float outputs[MAX_ACTUATORS] {};
int num_outputs{0}; int num_outputs{0};
int num_updates{0}; int num_updates{0};
bool was_scheduled{false}; bool was_scheduled{false};
@ -454,10 +453,10 @@ TEST_F(MixerModuleTest, prearm)
for (int i = 0; i < max_num_outputs; ++i) { for (int i = 0; i < max_num_outputs; ++i) {
if (i == 1) { if (i == 1) {
EXPECT_EQ(test_module.outputs[i], max_value); EXPECT_FLOAT_EQ(test_module.outputs[i], max_value);
} else { } else {
EXPECT_EQ(test_module.outputs[i], disarmed_value); EXPECT_FLOAT_EQ(test_module.outputs[i], disarmed_value);
} }
} }

3
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp
View File

@ -60,8 +60,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
return true; 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; unsigned active_output_count = 0;

3
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp
View File

@ -56,8 +56,7 @@ public:
_node_mutex(node_mutex) _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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; } MixingOutput &mixingOutput() { return _mixing_output; }

10
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
View File

@ -53,22 +53,20 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
return true; 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; bool updated = false;
// cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
int i = 0; int i = 0;
for (auto &servo_pub : _servos_pub) { for (auto &servo_pub : _servos_pub) {
if (_mixing_output.isFunctionSet(i)) { if (_mixing_output.isFunctionSet(i)) {
gz::msgs::Double servo_output; gz::msgs::Double servo_output{};
///TODO: Normalize output data ///TODO: Normalize output data
double output = (outputs[i] - 500) / 500.0;
// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
// std::cout << " output: " << output << std::endl; // std::cout << " output: " << output << std::endl;
servo_output.set_data(output); servo_output.set_data(outputs[i]);
if (servo_pub.Valid()) { if (servo_pub.Valid()) {
servo_pub.Publish(servo_output); servo_pub.Publish(servo_output);

3
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
View File

@ -49,8 +49,7 @@ public:
_node_mutex(node_mutex) _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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; } MixingOutput &mixingOutput() { return _mixing_output; }

16
src/modules/simulation/gz_bridge/module.yaml
View File

@ -11,17 +11,17 @@ actuator_output:
group_label: 'ESCs' group_label: 'ESCs'
channel_label: 'ESC' channel_label: 'ESC'
standard_params: standard_params:
disarmed: { min: 0, max: 1000, default: 0 } disarmed: { min: -8192, max: 8191, default: 0 }
min: { min: 0, max: 1000, default: 0 } min: { min: -8192, max: 8191, default: 0 }
max: { min: 0, max: 1000, default: 1000 } max: { min: -8192, max: 8191, default: 8191 }
failsafe: { min: 0, max: 1000 } failsafe: { min: -8192, max: 8191 }
num_channels: 8 num_channels: 8
- param_prefix: SIM_GZ_SV - param_prefix: SIM_GZ_SV
group_label: 'Servos' group_label: 'Servos'
channel_label: 'Servo' channel_label: 'Servo'
standard_params: standard_params:
disarmed: { min: 0, max: 1000, default: 500 } disarmed: { min: -3.14159, max: 3.14159, default: 0 }
min: { min: 0, max: 1000, default: 0 } min: { min: -3.14159, max: 3.14159, default: -3.14159 }
max: { min: 0, max: 1000, default: 1000 } max: { min: -3.14159, max: 3.14159, default: 3.14159 }
failsafe: { min: 0, max: 1000 } failsafe: { min: -3.14159, max: 3.14159 }
num_channels: 8 num_channels: 8

10
src/modules/simulation/pwm_out_sim/PWMSim.cpp
View File

@ -58,18 +58,15 @@ PWMSim::~PWMSim()
perf_free(_interval_perf); 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)
if (num_control_groups_updated > 0) {
actuator_outputs_s actuator_outputs{}; actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = num_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++) { 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); OutputFunction function = _mixing_output.outputFunction(i);
bool is_reversible = reversible_outputs & (1u << i); bool is_reversible = reversible_outputs & (1u << i);
@ -92,9 +89,6 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un
actuator_outputs.timestamp = hrt_absolute_time(); actuator_outputs.timestamp = hrt_absolute_time();
_actuator_outputs_sim_pub.publish(actuator_outputs); _actuator_outputs_sim_pub.publish(actuator_outputs);
return true; return true;
}
return false;
} }
void PWMSim::Run() void PWMSim::Run()

3
src/modules/simulation/pwm_out_sim/PWMSim.hpp
View File

@ -70,8 +70,7 @@ public:
/** @see ModuleBase::print_status() */ /** @see ModuleBase::print_status() */
int print_status() override; 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) override;
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:
void Run() override; void Run() override;