Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-27 09:33:51 +08:00
Code Issues Actions 11 Packages Projects Releases Wiki Activity

mixer: remove used flag stop_outputs

Browse Source 
dshot: fix motor test on CANnode

Also includes fixes for the DShot driver since stop_outputs is removed. The esc info command has been removed because it doesn't work with AM32, can only be used via command line, and complicates the driver
This commit is contained in:
Jacob Dahl
2025-07-09 12:39:16 -08:00
committed by Alex Klimaj
parent 6a1cefd7a6
commit 09ebd21e55
37 changed files with 120 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
View File

@@ -569,11 +569,11 @@ void process_capture_results(uint8_t timer_index, uint8_t channel_index)
}
/**
* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution)
* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
@@ -586,7 +586,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
uint16_t packet = 0;
uint16_t checksum = 0;
packet |= throttle << DSHOT_THROTTLE_POSITION;
packet |= data << DSHOT_THROTTLE_POSITION;
packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
uint16_t csum_data = packet;

2
src/drivers/actuators/vertiq_io/vertiq_io.cpp
View File

@@ -195,7 +195,7 @@ void VertiqIo::OutputControls(uint16_t outputs[MAX_ACTUATORS])
_serial_interface.ProcessSerialTx();
}
bool VertiqIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool VertiqIo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
#ifdef CONFIG_USE_IFCI_CONFIGURATION

2
src/drivers/actuators/vertiq_io/vertiq_io.hpp
View File

@@ -94,7 +94,7 @@ public:
void print_info();
/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
/**

4
src/drivers/actuators/voxl_esc/voxl_esc.cpp
View File

@@ -1196,7 +1196,7 @@ void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
}
/* OutputModuleInterface */
bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
//in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function)
@@ -1212,7 +1212,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
if (!_outputs_on || stop_motors) {
if (!_outputs_on) {
_esc_chans[i].rate_req = 0;
} else {

2
src/drivers/actuators/voxl_esc/voxl_esc.hpp
View File

@@ -81,7 +81,7 @@ public:
void print_params();
/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
virtual int init();

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

@@ -169,7 +169,7 @@ public:
{
}
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
return;

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

@@ -435,7 +435,7 @@ void CyphalNode::sendPortList()
_uavcan_node_port_List_last = now;
}
bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool UavcanMixingInterface::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
// Note: This gets called from MixingOutput from within its update() function
@@ -445,7 +445,7 @@ bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX
auto publisher = static_cast<UavcanEscController *>(_pub_manager.getPublisher("esc"));
if (publisher) {
publisher->update_outputs(stop_motors, outputs, num_outputs);
publisher->update_outputs(outputs, num_outputs);
}
return true;

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

@@ -87,7 +87,7 @@ public:
_node_mutex(node_mutex),
_pub_manager(pub_manager) {}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void printInfo() { _mixing_output.printStatus(); }

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

@@ -58,6 +58,9 @@ DShot::~DShot()
up_dshot_arm(false);
perf_free(_cycle_perf);
perf_free(_bdshot_rpm_perf);
perf_free(_dshot_telem_perf);
delete _telemetry;
}
@@ -248,6 +251,8 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
_last_telemetry_index = telemetry_index;
perf_count(_dshot_telem_perf);
return ret;
}
@@ -323,10 +328,10 @@ int DShot::handle_new_bdshot_erpm(void)
++telemetry_index;
}
}
perf_count(_bdshot_rpm_perf);
return num_erpms;
}
@@ -362,59 +367,12 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num
return 0;
}
void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index)
{
if (_request_esc_info.load() != nullptr) {
// already in progress (not expected to ever happen)
return;
}
DShotTelemetry::OutputBuffer output_buffer{};
output_buffer.motor_index = motor_index;
// start the request
_request_esc_info.store(&output_buffer);
// wait until processed
int max_time = 1000;
while (_request_esc_info.load() != nullptr && max_time-- > 0) {
px4_usleep(1000);
}
_request_esc_info.store(nullptr); // just in case we time out...
if (output_buffer.buf_pos == 0) {
PX4_ERR("No data received. If telemetry is setup correctly, try again");
return;
}
DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer);
}
int DShot::request_esc_info()
{
_telemetry->redirectOutput(*_request_esc_info.load());
_waiting_for_esc_info = true;
int motor_index = _request_esc_info.load()->motor_index;
_current_command.motor_mask = 1 << motor_index;
_current_command.num_repetitions = 1;
_current_command.command = DShot_cmd_esc_info;
_current_command.save = false;
PX4_DEBUG("Requesting ESC info for motor %i", motor_index);
return motor_index;
}
void DShot::mixerChanged()
{
update_num_motors();
}
bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
if (!_outputs_on) {
@@ -424,93 +382,47 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
int requested_telemetry_index = -1;
if (_telemetry) {
// check for an ESC info request. We only process it when we're not expecting other telemetry data
if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors
&& !_telemetry->expectingData() && !_current_command.valid()) {
requested_telemetry_index = request_esc_info();
} else {
requested_telemetry_index = _telemetry->getRequestMotorIndex();
}
requested_telemetry_index = _telemetry->getRequestMotorIndex();
}
if (stop_motors) {
int telemetry_index = 0;
int telemetry_index = 0;
for (int i = 0; i < (int)num_outputs; i++) {
uint16_t output = outputs[i];
if (output == DSHOT_DISARM_VALUE) {
// when motors are stopped we check if we have other commands to send
for (int i = 0; i < (int)num_outputs; i++) {
if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) {
// for some reason we need to always request telemetry when sending a command
up_dshot_motor_command(i, _current_command.command, true);
} else {
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
}
telemetry_index += _mixing_output.isFunctionSet(i);
}
} else {
if (_current_command.valid()) {
--_current_command.num_repetitions;
if (_current_command.num_repetitions == 0 && _current_command.save) {
_current_command.save = false;
_current_command.num_repetitions = 10;
_current_command.command = dshot_command_t::DShot_cmd_save_settings;
}
}
} else {
int telemetry_index = 0;
for (int i = 0; i < (int)num_outputs; i++) {
uint16_t output = outputs[i];
if (output == DSHOT_DISARM_VALUE) {
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
} else {
// DShot 3D splits the throttle ranges in two.
// This is in terms of DShot values, code below is in terms of actuator_output
// Direction 1) 48 is the slowest, 1047 is the fastest.
// Direction 2) 1049 is the slowest, 2047 is the fastest.
if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
output = DSHOT_DISARM_VALUE;
} else {
bool upper_range = output >= 1000;
if (upper_range) {
output -= 1000;
} else {
output = 999 - output; // lower range is inverted
}
float max_output = 999.f;
float min_output = max_output * _param_dshot_min.get();
output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
if (upper_range) {
output += 1000;
}
}
}
up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
telemetry_index == requested_telemetry_index);
if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
output = convert_output_to_3d_scaling(output);
}
telemetry_index += _mixing_output.isFunctionSet(i);
up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
telemetry_index == requested_telemetry_index);
}
// clear commands when motors are running
_current_command.clear();
telemetry_index += _mixing_output.isFunctionSet(i);
}
// Decrement the command counter
if (_current_command.valid()) {
--_current_command.num_repetitions;
// Queue a save command after the burst if save has been requested
if (_current_command.num_repetitions == 0 && _current_command.save) {
_current_command.save = false;
_current_command.num_repetitions = 10;
_current_command.command = dshot_command_t::DShot_cmd_save_settings;
}
}
up_dshot_trigger();
@@ -518,6 +430,36 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
return true;
}
uint16_t DShot::convert_output_to_3d_scaling(uint16_t output)
{
// DShot 3D splits the throttle ranges in two.
// This is in terms of DShot values, code below is in terms of actuator_output
// Direction 1) 48 is the slowest, 1047 is the fastest.
// Direction 2) 1049 is the slowest, 2047 is the fastest.
if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
return DSHOT_DISARM_VALUE;
}
bool upper_range = output >= 1000;
if (upper_range) {
output -= 1000;
} else {
output = 999 - output; // lower range is inverted
}
float max_output = 999.f;
float min_output = max_output * _param_dshot_min.get();
output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
if (upper_range) {
output += 1000;
}
return output;
}
void DShot::Run()
{
if (should_exit()) {
@@ -542,14 +484,7 @@ void DShot::Run()
if (_telemetry) {
const int telem_update = _telemetry->update(_num_motors);
// Are we waiting for ESC info?
if (_waiting_for_esc_info) {
if (telem_update != -1) {
_request_esc_info.store(nullptr);
_waiting_for_esc_info = false;
}
} else if (telem_update >= 0) {
if (telem_update >= 0) {
const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(),
_bidirectional_dshot_enabled);
@@ -772,27 +707,6 @@ int DShot::custom_command(int argc, char *argv[])
}
}
if (!strcmp(verb, "esc_info")) {
if (!is_running()) {
PX4_ERR("module not running");
return -1;
}
if (motor_index == -1) {
PX4_ERR("No motor index specified");
return -1;
}
if (!get_instance()->telemetry_enabled()) {
PX4_ERR("Telemetry is not enabled, but required to get ESC info");
return -1;
}
get_instance()->retrieve_and_print_esc_info_thread_safe(motor_index);
return 0;
}
if (!is_running()) {
int ret = DShot::task_spawn(argc, argv);
@@ -810,6 +724,9 @@ int DShot::print_status()
PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
perf_print_counter(_cycle_perf);
perf_print_counter(_bdshot_rpm_perf);
perf_print_counter(_dshot_telem_perf);
_mixing_output.printStatus();
if (_telemetry) {
@@ -881,9 +798,6 @@ After saving, the reversed direction will be regarded as the normal one. So to r
PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5");
PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("esc_info", "Request ESC information");
PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;

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

@@ -76,8 +76,6 @@ public:
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void retrieve_and_print_esc_info_thread_safe(const int motor_index);
/**
* Send a dshot command to one or all motors
* This is expected to be called from another thread.
@@ -92,7 +90,7 @@ public:
bool telemetry_enabled() const { return _telemetry != nullptr; }
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
@@ -131,8 +129,6 @@ private:
int handle_new_bdshot_erpm(void);
int request_esc_info();
void Run() override;
void update_params();
@@ -141,6 +137,8 @@ private:
void handle_vehicle_commands();
uint16_t convert_output_to_3d_scaling(uint16_t output);
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uint32_t _reversible_outputs{};
@@ -154,11 +152,9 @@ private:
px4::atomic<Command *> _new_command{nullptr};
px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
bool _outputs_initialized{false};
bool _outputs_on{false};
bool _waiting_for_esc_info{false};
bool _bidirectional_dshot_enabled{false};
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
@@ -167,6 +163,8 @@ private:
int _num_motors{0};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")};
perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")};
Command _current_command{};

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

@@ -95,7 +95,7 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool LinuxPWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
_pwm_out->send_output_pwm(outputs, num_outputs);

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

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

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

@@ -70,7 +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,
bool updateOutputs(uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated) override;
int print_status() override;
@@ -136,7 +136,7 @@ int PCA9685Wrapper::init()
return PX4_OK;
}
bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated)
{
if (state != STATE::RUNNING) { return false; }

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

@@ -125,7 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on)
return true;
}
bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool PWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
/* output to the servos */

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

@@ -71,7 +71,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:

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

@@ -153,7 +153,7 @@ public:
uint16_t system_status() const { return _status; }
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated) override;
private:
@@ -360,7 +360,7 @@ PX4IO::~PX4IO()
perf_free(_interface_write_perf);
}
bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool PX4IO::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
for (size_t i = 0; i < num_outputs; i++) {

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

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

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

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

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

@@ -241,7 +241,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet)
tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1);
}
bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool TAP_ESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
if (_initialized) {

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

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

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

@@ -85,10 +85,8 @@ UavcanEscController::init()
}
void
UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs)
UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs)
{
// TODO: remove stop_motors as it's unnecessary and unused almost everywhere
// TODO: configurable rate limit
const auto timestamp = _node.getMonotonicTime();

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 total_outputs);
void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs);
/**
* Sets the number of rotors and enable timer

2
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(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
uavcan::equipment::actuator::ArrayCommand msg;

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(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
private:
uavcan::INode &_node;

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

@@ -1012,10 +1012,10 @@ UavcanNode::Run()
}
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
_esc_controller.update_outputs(outputs, num_outputs);
return true;
}
@@ -1042,10 +1042,10 @@ void UavcanMixingInterfaceESC::mixerChanged()
_esc_controller.set_rotor_count(rotor_count);
}
bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool UavcanMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
_servo_controller.update_outputs(stop_motors, outputs, num_outputs);
_servo_controller.update_outputs(outputs, num_outputs);
return true;
}

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

@@ -127,7 +127,7 @@ public:
_node_mutex(node_mutex),
_esc_controller(esc_controller) {}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override;
@@ -158,7 +158,7 @@ public:
_node_mutex(node_mutex),
_servo_controller(servo_controller) {}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; }

10
src/drivers/voxl2_io/voxl2_io.cpp
View File

@@ -276,7 +276,7 @@ int Voxl2IO::get_version_info()
return (got_response == true ? 0 : -1);
}
bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
// Stop Mixer while ESCs are being calibrated
@@ -305,13 +305,7 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN
_pwm_on = true;
}
//Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right?
if (0) { //(!_pwm_on || stop_motors) {
output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns
} else {
output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns
}
output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns
}
Command cmd;

2
src/drivers/voxl2_io/voxl2_io.hpp
View File

@@ -82,7 +82,7 @@ public:
int print_status() override;
/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
bool updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
virtual int init();

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

@@ -473,16 +473,12 @@ bool MixingOutput::update()
void
MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates)
{
bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode();
if (_armed.lockdown || _armed.manual_lockdown) {
// overwrite outputs in case of lockdown with disarmed values
for (size_t i = 0; i < _max_num_outputs; i++) {
_current_output_value[i] = _disarmed_value[i];
}
stop_motors = true;
} else if (_armed.force_failsafe) {
// overwrite outputs in case of force_failsafe with _failsafe_value values
for (size_t i = 0; i < _max_num_outputs; i++) {
@@ -513,7 +509,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
}
/* now return the outputs to the driver */
if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) {
if (_interface.updateOutputs(_current_output_value, _max_num_outputs, has_updates)) {
actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(_max_num_outputs, actuator_outputs);

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

@@ -79,14 +79,12 @@ public:
/**
* Callback to update the (physical) actuator outputs in the driver
* @param stop_motors if true, all motors must be stopped (if false, individual motors
* 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],
virtual bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) = 0;
/** called whenever the mixer gets updated/reset */

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

@@ -86,7 +86,7 @@ public:
was_scheduled = true;
}
bool updateOutputs(bool stop_motors, uint16_t outputs_[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs_[MAX_ACTUATORS],
unsigned num_outputs_, unsigned num_control_groups_updated) override
{
memcpy(outputs, outputs_, sizeof(outputs));

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

@@ -62,7 +62,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
return true;
}
bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool GZMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
unsigned active_output_count = 0;

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

@@ -55,7 +55,7 @@ public:
_node(node)
{}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; }

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

@@ -128,7 +128,7 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
return true;
}
bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool GZMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
bool updated = false;

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

@@ -49,7 +49,7 @@ public:
_node(node)
{}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; }

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

@@ -58,7 +58,7 @@ PWMSim::~PWMSim()
perf_free(_interval_perf);
}
bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool PWMSim::updateOutputs(uint16_t 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)

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

@@ -70,7 +70,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: