Skip to content

Commit

Permalink
dshot: avoid duplicate publications for bidir and telem
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@julianoes @AlexKlimaj
julianoes authored and AlexKlimaj committed Nov 26, 2024
1 parent 4cf2f24 commit fdb94f9
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 69 deletions.
99 changes: 51 additions & 48 deletions src/drivers/dshot/DShot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -182,96 +178,95 @@ 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");
return;
}
}

_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<int>(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<int>(data.erpm) * 100) /
(_param_mot_pole_count.get() / 2);
}

esc_status.esc[telemetry_index].esc_voltage = static_cast<float>(data.voltage) * 0.01f;
esc_status.esc[telemetry_index].esc_current = static_cast<float>(data.current) * 0.01f;
esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
// TODO: accumulate consumption and use for battery estimation
}

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

Expand All @@ -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;
Expand All @@ -302,15 +301,15 @@ 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++;
esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
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;
}

Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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],
Expand All @@ -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();
}
}

Expand Down Expand Up @@ -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) {
Expand All @@ -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();
}
Expand Down Expand Up @@ -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 */
Expand Down
18 changes: 9 additions & 9 deletions src/drivers/dshot/DShot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,18 +120,14 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
void clear() { num_repetitions = 0; }
};

struct Telemetry {
DShotTelemetry handler{};
uORB::PublicationMultiData<esc_status_s> 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);

Expand All @@ -143,14 +139,16 @@ class DShot final : public ModuleBase<DShot>, 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_s> esc_status_pub{ORB_ID(esc_status)};

static char _telemetry_device[20];
static px4::atomic_bool _request_telemetry_init;
Expand All @@ -167,6 +165,8 @@ class DShot final : public ModuleBase<DShot>, 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{};
Expand Down
12 changes: 6 additions & 6 deletions src/drivers/dshot/DShotTelemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -120,7 +120,7 @@ int DShotTelemetry::update()
++_num_timeouts;
}

requestNextMotor();
requestNextMotor(num_motors);
return -2;
}

Expand All @@ -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 {
Expand All @@ -153,7 +153,7 @@ int DShotTelemetry::update()
ret = _current_motor_index_request;
}

requestNextMotor();
requestNextMotor(num_motors);
}
}
}
Expand Down Expand Up @@ -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;
}
Expand Down
9 changes: 3 additions & 6 deletions src/drivers/dshot/DShotTelemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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
Expand All @@ -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};

Expand Down

0 comments on commit fdb94f9

Please sign in to comment.