forked from Archive/PX4-Autopilot
WIP: output modules native units
This commit is contained in:
parent
d259386987
commit
3d7f6ea09c
|
@ -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,
|
||||||
|
|
|
@ -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_OUTPUT_GROUPS = 4 # for sanity checking
|
uint8 NUM_ACTUATOR_OUTPUTS = 16
|
||||||
uint32 noutputs # valid outputs
|
uint8 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])
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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();
|
||||||
};
|
};
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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(); }
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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:
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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]);
|
||||||
|
|
|
@ -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.
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
up_pwm_update(_pwm_mask);
|
||||||
* the oneshots with updated values.
|
|
||||||
*/
|
|
||||||
if (num_control_groups_updated > 0) {
|
|
||||||
up_pwm_update(_pwm_mask);
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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] {};
|
||||||
|
|
|
@ -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:
|
||||||
|
|
||||||
|
|
|
@ -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 {
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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,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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
||||||
|
|
|
@ -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];
|
||||||
|
|
|
@ -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 {
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -58,43 +58,37 @@ 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)
|
actuator_outputs_s actuator_outputs{};
|
||||||
if (num_control_groups_updated > 0) {
|
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++) {
|
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);
|
||||||
float output = outputs[i];
|
float output = outputs[i];
|
||||||
|
|
||||||
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
|
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
|
||||||
&& !is_reversible) {
|
&& !is_reversible) {
|
||||||
// Scale non-reversible motors to [0, 1]
|
// 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);
|
actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Scale everything else to [-1, 1]
|
// 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_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;
|
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.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()
|
void PWMSim::Run()
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue