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:
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,

6
msg/ActuatorOutputs.msg
View File

@ -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])

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_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;

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

@ -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();
};

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) {
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<float>(outputs[i]);
msg_sp.value[i] = outputs[i];
} else {
// "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,
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

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

@ -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(); }

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

@ -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);

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

@ -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:

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;
}
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;
}

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

@ -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;

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

@ -34,8 +34,10 @@
#include <px4_log.h>
#include <px4_defines.h>
#include <cmath>
#include "PCA9685.h"
#include <lib/mathlib/mathlib.h>
#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]);

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

@ -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.

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 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);
}
}

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

@ -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;
}

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

@ -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;

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

@ -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;

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],
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);

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

@ -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);

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);
}
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] {};

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

@ -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:

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

@ -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<unsigned>(0));
} else {

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

@ -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

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

@ -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);
}

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

@ -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;

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

@ -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

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

@ -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;

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

@ -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; }

74
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();
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<uint16_t>(_current_output_value[i]) == static_cast<uint16_t>(_min_value[i])) {
_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;
}
}
}
/* 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<float>(diff) / RAMP_TIME_US;
if (progress > 1.f) {
progress = 1.f;
}
float elapsed_us = static_cast<float>(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];

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

@ -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 {

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

@ -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);
}
}

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

@ -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;

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

@ -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; }

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

@ -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);

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

@ -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; }

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

@ -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

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

@ -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()

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

@ -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;