From 952a3956a9cd2ac29c74356de2f42fe95ece9ae3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Aug 2020 12:53:19 +1000 Subject: [PATCH] AP_BLHeli: added have_telem_data() API --- libraries/AP_BLHeli/AP_BLHeli.cpp | 1 + libraries/AP_BLHeli/AP_BLHeli.h | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index fc04a5af5a..5f376a3608 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1417,6 +1417,7 @@ void AP_BLHeli::read_telemetry_packet(void) last_telem[last_telem_esc] = td; last_telem[last_telem_esc].count++; + received_telem_data = true; AP_Logger *logger = AP_Logger::get_singleton(); if (logger && logger->logging_enabled() diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index aab0909ce0..881cf39496 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -65,6 +65,11 @@ public: // return all of the motor frequencies in Hz for dynamic filtering uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const; + // return true if we have received any telemetry data + bool have_telem_data(void) const { + return received_telem_data; + } + static AP_BLHeli *get_singleton(void) { return _singleton; } @@ -228,6 +233,8 @@ private: uint8_t num_motors; struct telem_data last_telem[max_motors]; + uint32_t received_telem_data; + // last log output to avoid beat frequencies uint32_t last_log_ms[max_motors];