diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 3582dab43c..5fc7291db2 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -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(); } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 2dcab5696f..e60b33b862 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -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")};