fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

dshot: refactoring

Browse Source
This commit is contained in:
Peter van der Perk 2024-05-23 13:18:25 +02:00 committed by David Sidrane
parent ff6966da57
commit 7982f54a6a
2 changed files with 14 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -144,9 +144,9 @@ void DShot::enable_dshot_outputs(const bool enabled)
}
}
_bdshot = _param_bidirectional_enable.get();
_bidirectional_dshot_enabled = _param_bidirectional_enable.get();
int ret = up_dshot_init(_output_mask, dshot_frequency, _bdshot);
int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
@ -160,8 +160,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
if (((1 << i) & _output_mask) == 0) {
_mixing_output.disableFunction(i);
} else {
_dshot_esc_count++;
}
}
@ -173,7 +171,7 @@ void DShot::enable_dshot_outputs(const bool enabled)
_outputs_initialized = true;
if (_bdshot) {
if (_bidirectional_dshot_enabled) {
init_telemetry(NULL);
}
}
@ -263,12 +261,11 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
void DShot::publish_esc_status(void)
{
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
uint8_t channel;
// clear data of the esc that are offline
for (channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
if ((esc_status.esc_online_flags & (1 << channel)) == 0) {
memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s));
for (int index = 0; (index < _telemetry->last_telemetry_index); index++) {
if ((esc_status.esc_online_flags & (1 << index)) == 0) {
memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s));
}
}
@ -276,14 +273,14 @@ void DShot::publish_esc_status(void)
esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
if (_bdshot) {
if (_bidirectional_dshot_enabled) {
esc_status.esc_online_flags |= _output_mask;
esc_status.esc_armed_flags |= _output_mask;
esc_status.esc_count = _dshot_esc_count;
esc_status.esc_count = _telemetry->handler.numMotors();
for (channel = 0; (channel < 8); channel++) {
if (up_bdshot_channel_status(channel) == 0) {
esc_status.esc_online_flags &= ~(1 << channel);
for (int index = 0; (index < esc_status.esc_count); index++) {
if (up_bdshot_channel_status(index) == 0) {
esc_status.esc_online_flags &= ~(1 << index);
}
}
}
@ -544,7 +541,7 @@ void DShot::Run()
need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
}
if (_bdshot) {
if (_bidirectional_dshot_enabled) {
// Add bdshot data to esc status
need_to_publish += handle_new_bdshot_erpm();
}
@ -794,7 +791,7 @@ int DShot::print_status()
}
/* Print dshot status */
if (_bdshot) {
if (_bidirectional_dshot_enabled) {
up_bdshot_status();
}

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

@ -162,11 +162,10 @@ private:
bool _outputs_initialized{false};
bool _outputs_on{false};
bool _waiting_for_esc_info{false};
bool _bdshot{false};
bool _bidirectional_dshot_enabled{false};
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
uint32_t _output_mask{0};
uint32_t _dshot_esc_count{0};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};