From fdb94f938c03b93a3166cf8dbf3b79c079f287a3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 15:16:44 +1300 Subject: [PATCH] dshot: avoid duplicate publications for bidir and telem Instead of publishing both bidirectional dshot updates as well as telemetry updates, we now combine the data from both streams, and publish whenever we get RPM updates, as the latter arrives with higher rate, e.g. 200 Hz with round robin, or faster otherwise. When combining the data, we take RPM from bidirectional dshot, and the rest from telemetry. When we have only one of the two, either telemetry or bidirectional dshot, we just publish that one. --- src/drivers/dshot/DShot.cpp | 99 ++++++++++++++-------------- src/drivers/dshot/DShot.h | 18 ++--- src/drivers/dshot/DShotTelemetry.cpp | 12 ++-- src/drivers/dshot/DShotTelemetry.h | 9 +-- 4 files changed, 69 insertions(+), 69 deletions(-) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index a898948d61ff..66cdeee98954 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -170,10 +170,6 @@ void DShot::enable_dshot_outputs(const bool enabled) } _outputs_initialized = true; - - if (_bidirectional_dshot_enabled) { - init_telemetry(NULL); - } } if (_outputs_initialized) { @@ -182,28 +178,24 @@ void DShot::enable_dshot_outputs(const bool enabled) } } -void DShot::update_telemetry_num_motors() +void DShot::update_num_motors() { - if (!_telemetry) { - return; - } - int motor_count = 0; for (unsigned i = 0; i < _num_outputs; ++i) { if (_mixing_output.isFunctionSet(i)) { - _telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); + _actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); ++motor_count; } } - _telemetry->handler.setNumMotors(motor_count); + _num_motors = motor_count; } void DShot::init_telemetry(const char *device) { if (!_telemetry) { - _telemetry = new Telemetry{}; + _telemetry = new DShotTelemetry{}; if (!_telemetry) { PX4_ERR("alloc failed"); @@ -211,32 +203,35 @@ void DShot::init_telemetry(const char *device) } } - _telemetry->esc_status_pub.advertise(); - if (device != NULL) { - int ret = _telemetry->handler.init(device); + int ret = _telemetry->init(device); if (ret != 0) { PX4_ERR("telemetry init failed (%i)", ret); } } - update_telemetry_num_motors(); + update_num_motors(); } -int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) +int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm) { int ret = 0; // fill in new motor data - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) { esc_status.esc_online_flags |= 1 << telemetry_index; - esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; - esc_status.esc[telemetry_index].timestamp = data.time; - esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / - (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; + + if (!ignore_rpm) { + // If we also have bidirectional dshot, we use rpm and timestamps from there. + esc_status.esc[telemetry_index].timestamp = data.time; + esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / + (_param_mot_pole_count.get() / 2); + } + esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f; esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f; esc_status.esc[telemetry_index].esc_temperature = static_cast(data.temperature); @@ -244,34 +239,34 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem } // publish when motor index wraps (which is robust against motor timeouts) - if (telemetry_index <= _telemetry->last_telemetry_index) { + if (telemetry_index <= _last_telemetry_index) { esc_status.timestamp = hrt_absolute_time(); esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; - esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_count = _num_motors; ++esc_status.counter; ret = 1; // Indicate we wrapped, so we publish data } - _telemetry->last_telemetry_index = telemetry_index; + _last_telemetry_index = telemetry_index; return ret; } void DShot::publish_esc_status(void) { - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); int telemetry_index = 0; // clear data of the esc that are offline - for (int index = 0; (index < _telemetry->last_telemetry_index); index++) { + for (int index = 0; (index < _last_telemetry_index); index++) { if ((esc_status.esc_online_flags & (1 << index)) == 0) { memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s)); } } // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_count = _num_motors; esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; @@ -290,8 +285,12 @@ void DShot::publish_esc_status(void) } } - // ESC telem wrap around or bdshot update - _telemetry->esc_status_pub.update(); + if (!esc_status_pub.advertised()) { + esc_status_pub.advertise(); + + } else { + esc_status_pub.update(); + } // reset esc online flags esc_status.esc_online_flags = 0; @@ -302,7 +301,7 @@ int DShot::handle_new_bdshot_erpm(void) int num_erpms = 0; int telemetry_index = 0; int erpm; - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); esc_status.timestamp = hrt_absolute_time(); esc_status.counter = _esc_status_counter++; @@ -310,7 +309,7 @@ int DShot::handle_new_bdshot_erpm(void) esc_status.esc_armed_flags = _outputs_on; // We wait until all are ready. - if (up_bdshot_num_erpm_ready() < (int)popcount(_output_mask)) { + if (up_bdshot_num_erpm_ready() < _num_motors) { return 0; } @@ -321,7 +320,7 @@ int DShot::handle_new_bdshot_erpm(void) esc_status.esc_online_flags |= 1 << telemetry_index; esc_status.esc[telemetry_index].timestamp = hrt_absolute_time(); esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); - esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; } ++telemetry_index; @@ -397,7 +396,7 @@ void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index) int DShot::request_esc_info() { - _telemetry->handler.redirectOutput(*_request_esc_info.load()); + _telemetry->redirectOutput(*_request_esc_info.load()); _waiting_for_esc_info = true; int motor_index = _request_esc_info.load()->motor_index; @@ -413,7 +412,8 @@ int DShot::request_esc_info() void DShot::mixerChanged() { - update_telemetry_num_motors(); + update_num_motors(); + } bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -428,11 +428,11 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], 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->handler.expectingData() && !_current_command.valid()) { + && !_telemetry->expectingData() && !_current_command.valid()) { requested_telemetry_index = request_esc_info(); } else { - requested_telemetry_index = _telemetry->handler.getRequestMotorIndex(); + requested_telemetry_index = _telemetry->getRequestMotorIndex(); } } @@ -542,8 +542,7 @@ void DShot::Run() } if (_telemetry) { - int telem_update = _telemetry->handler.update(); - int need_to_publish = 0; + const int telem_update = _telemetry->update(_num_motors); // Are we waiting for ESC info? if (_waiting_for_esc_info) { @@ -553,21 +552,25 @@ void DShot::Run() } } else if (telem_update >= 0) { - need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); - } + const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(), + _bidirectional_dshot_enabled); - if (_bidirectional_dshot_enabled) { - // Add bdshot data to esc status - need_to_publish += handle_new_bdshot_erpm(); + // We don't want to publish twice, once by telemetry and once by bidirectional dishot. + if (!_bidirectional_dshot_enabled && need_to_publish) { + publish_esc_status(); + } } + } - if (need_to_publish > 0) { - // ESC telem wrap around or bdshot update + if (_bidirectional_dshot_enabled) { + // Add bdshot data to esc status + const int need_to_publish = handle_new_bdshot_erpm(); + + if (need_to_publish) { publish_esc_status(); } } - if (_parameter_update_sub.updated()) { update_params(); } @@ -802,7 +805,7 @@ int DShot::print_status() if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); - _telemetry->handler.printStatus(); + _telemetry->printStatus(); } /* Print dshot status */ diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e60b33b86279..e744c27f2601 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -120,18 +120,14 @@ class DShot final : public ModuleBase, public OutputModuleInterface void clear() { num_repetitions = 0; } }; - struct Telemetry { - DShotTelemetry handler{}; - uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; - int last_telemetry_index{-1}; - uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; - }; + int _last_telemetry_index{-1}; + uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; void enable_dshot_outputs(const bool enabled); void init_telemetry(const char *device); - int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm); void publish_esc_status(void); @@ -143,14 +139,16 @@ class DShot final : public ModuleBase, public OutputModuleInterface void update_params(); - void update_telemetry_num_motors(); + void update_num_motors(); void handle_vehicle_commands(); MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; - Telemetry *_telemetry{nullptr}; + DShotTelemetry *_telemetry{nullptr}; + + uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; static char _telemetry_device[20]; static px4::atomic_bool _request_telemetry_init; @@ -167,6 +165,8 @@ class DShot final : public ModuleBase, public OutputModuleInterface static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; + int _num_motors{0}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; Command _current_command{}; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 0132a9b04941..70992b1703ca 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -87,7 +87,7 @@ int DShotTelemetry::redirectOutput(OutputBuffer &buffer) return 0; } -int DShotTelemetry::update() +int DShotTelemetry::update(int num_motors) { if (_uart_fd < 0) { return -1; @@ -120,7 +120,7 @@ int DShotTelemetry::update() ++_num_timeouts; } - requestNextMotor(); + requestNextMotor(num_motors); return -2; } @@ -142,7 +142,7 @@ int DShotTelemetry::update() _redirect_output = nullptr; ret = _current_motor_index_request; _current_motor_index_request = -1; - requestNextMotor(); + requestNextMotor(num_motors); } } else { @@ -153,7 +153,7 @@ int DShotTelemetry::update() ret = _current_motor_index_request; } - requestNextMotor(); + requestNextMotor(num_motors); } } } @@ -225,9 +225,9 @@ uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len) } -void DShotTelemetry::requestNextMotor() +void DShotTelemetry::requestNextMotor(int num_motors) { - _current_motor_index_request = (_current_motor_index_request + 1) % _num_motors; + _current_motor_index_request = (_current_motor_index_request + 1) % num_motors; _current_request_start = 0; _frame_position = 0; } diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index 1164d1e2008c..8672e85f7f48 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -64,14 +64,12 @@ class DShotTelemetry void deinit(); - void setNumMotors(int num_motors) { _num_motors = num_motors; } - int numMotors() const { return _num_motors; } - /** * Read telemetry from the UART (non-blocking) and handle timeouts. + * @param num_motors How many DShot enabled motors * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data. */ - int update(); + int update(int num_motors); /** * Redirect everything that is read into a different buffer. @@ -112,7 +110,7 @@ class DShotTelemetry */ int setBaudrate(unsigned baud); - void requestNextMotor(); + void requestNextMotor(int num_motors); /** * Decode a single byte from an ESC feedback frame @@ -126,7 +124,6 @@ class DShotTelemetry static uint8_t crc8(const uint8_t *buf, uint8_t len); int _uart_fd{-1}; - int _num_motors{0}; uint8_t _frame_buffer[ESC_FRAME_SIZE]; int _frame_position{0};