From f60fdde492d2206eb2ca130fbbb4e7192eb5993c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 23 Oct 2020 21:24:16 +0100 Subject: [PATCH] AP_BLHeli: return motor frequencies from bi-dir dshot if available add channel mask to cater for bi-dir dshot call into rcout to setup bi-dir dshot log bi-dir telemetry even when regular telemetry is disabled don't expose bi-dir dshot if not compiled in --- libraries/AP_BLHeli/AP_BLHeli.cpp | 101 +++++++++++++++++++++++++++--- libraries/AP_BLHeli/AP_BLHeli.h | 13 +++- 2 files changed, 104 insertions(+), 10 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 5f376a3608..e8d04e2932 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -117,7 +117,14 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 // @User: Advanced AP_GROUPINFO("REMASK", 10, AP_BLHeli, channel_reversible_mask, 0), - +#ifdef HAL_WITH_BIDIR_DSHOT + // @Param: BDMASK + // @DisplayName: BLHeli bitmask of bi-directional dshot channels + // @Description: Mask of channels which support bi-directional dshot. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch. + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @User: Advanced + AP_GROUPINFO("BDMASK", 11, AP_BLHeli, channel_bidir_dshot_mask, 0), +#endif AP_GROUPEND }; @@ -1293,7 +1300,10 @@ void AP_BLHeli::update(void) SRV_Channels::set_digital_mask(mask); SRV_Channels::set_reversible_mask(uint16_t(channel_reversible_mask.get()) & mask); hal.rcout->set_reversible_mask(channel_reversible_mask.get() & mask); - +#ifdef HAL_WITH_BIDIR_DSHOT + // possibly enable bi-directional dshot + hal.rcout->set_bidir_dshot_mask(channel_bidir_dshot_mask.get() & mask); +#endif // add motors from channel mask for (uint8_t i=0; i<16 && num_motors < max_motors; i++) { if (mask & (1U<get_erpm(i); + if (erpm > 0 && erpm < 0xFFFF) { + valid_escs++; + motor_freq += (erpm * 200 / motor_poles) / 60.f; + } + } else if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) { valid_escs++; // slew the update const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f); @@ -1356,9 +1372,15 @@ uint8_t AP_BLHeli::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const { const uint32_t now = AP_HAL::millis(); uint8_t valid_escs = 0; + // average the rpm of each motor as reported by BLHeli and convert to Hz for (uint8_t i = 0; i < num_motors && i < nfreqs; i++) { - if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) { + if (has_bidir_dshot(i)) { + uint16_t erpm = hal.rcout->get_erpm(i); + if (erpm > 0 && erpm < 0xFFFF) { + freqs[valid_escs++] = (erpm * 200 / motor_poles) / 60.f; + } + } else if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) { // slew the update const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f); freqs[valid_escs++] = (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f; @@ -1420,37 +1442,100 @@ void AP_BLHeli::read_telemetry_packet(void) received_telem_data = true; AP_Logger *logger = AP_Logger::get_singleton(); + + uint16_t trpm = td.rpm; + float terr = 0.0f; + if (logger && logger->logging_enabled() // log at 10Hz && td.timestamp_ms - last_log_ms[last_telem_esc] > 100) { + + if (has_bidir_dshot(last_telem_esc)) { + trpm = hal.rcout->get_erpm(last_telem_esc); + if (trpm != 0xFFFF) { + trpm = trpm * 200 / motor_poles; + } + terr = hal.rcout->get_erpm_error_rate(last_telem_esc); + } + logger->Write_ESC(uint8_t(last_telem_esc), AP_HAL::micros64(), - td.rpm*100U, + trpm*100U, td.voltage, td.current, td.temperature * 100U, td.consumption, - 0); + 0, + terr); last_log_ms[last_telem_esc] = td.timestamp_ms; } if (debug_level >= 2) { - hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u t=%u\n", + if (has_bidir_dshot(last_telem_esc)) { + trpm = hal.rcout->get_erpm(last_telem_esc); + if (trpm != 0xFFFF) { + trpm = trpm * 200 / motor_poles; + } + terr = hal.rcout->get_erpm_error_rate(last_telem_esc); + } + hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u e=%.1f t=%u\n", last_telem_esc, td.temperature, td.voltage, td.current, td.consumption, - td.rpm, (unsigned)AP_HAL::millis()); + trpm, terr, (unsigned)AP_HAL::millis()); } } +/* + log bidir telemetry - only called if BLH telemetry is not active + */ +void AP_BLHeli::log_bidir_telemetry(void) +{ + AP_Logger *logger = AP_Logger::get_singleton(); + + uint32_t now = AP_HAL::millis(); + + if (logger && logger->logging_enabled() + // log at 10Hz + && now - last_log_ms[last_telem_esc] > 100) { + + if (has_bidir_dshot(last_telem_esc)) { + uint16_t trpm = hal.rcout->get_erpm(last_telem_esc); + if (trpm != 0xFFFF) { + trpm = trpm * 200 / motor_poles; + } + float terr = hal.rcout->get_erpm_error_rate(last_telem_esc); + + logger->Write_ESC(uint8_t(last_telem_esc), + AP_HAL::micros64(), + trpm*100U, + 0, 0, 0, 0, + terr); + last_log_ms[last_telem_esc] = now; + + if (debug_level >= 2) { + hal.console->printf("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, terr, (unsigned)AP_HAL::millis()); + } + } + } + + last_telem_esc = (last_telem_esc + 1) % num_motors; +} + /* update BLHeli telemetry handling This is called on push() in SRV_Channels */ void AP_BLHeli::update_telemetry(void) { +#ifdef HAL_WITH_BIDIR_DSHOT + // we might only have bi-dir dshot + if (channel_bidir_dshot_mask.get() != 0 && !telem_uart) { + log_bidir_telemetry(); + } +#endif if (!telem_uart) { return; } diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 881cf39496..cce9682ee8 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -70,6 +70,12 @@ public: return received_telem_data; } + bool has_bidir_dshot(uint8_t esc_index) const { + return channel_bidir_dshot_mask.get() & (1 << esc_index); + } + + uint16_t get_bidir_dshot_mask() const { return channel_bidir_dshot_mask.get(); } + static AP_BLHeli *get_singleton(void) { return _singleton; } @@ -91,6 +97,8 @@ private: AP_Int8 output_type; AP_Int8 control_port; AP_Int8 motor_poles; + // mask of channels with bi-directional dshot enabled + AP_Int32 channel_bidir_dshot_mask; enum mspState { MSP_IDLE=0, @@ -214,7 +222,7 @@ private: ESC_PROTOCOL_ONESHOT125=2, ESC_PROTOCOL_DSHOT=5, }; - + // ESC status structure at address 0xEB00 struct PACKED esc_status { uint8_t unknown[3]; @@ -301,7 +309,8 @@ private: void run_connection_test(uint8_t chan); uint8_t telem_crc8(uint8_t crc, uint8_t crc_seed) const; void read_telemetry_packet(void); - + void log_bidir_telemetry(void); + // protocol handler hook bool protocol_handler(uint8_t , AP_HAL::UARTDriver *); };