diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index dcc46fdca1..f5bafac336 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -86,7 +86,7 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con float rpm; if (get_rpm(i, rpm)) { freqs[valid_escs++] = rpm * (1.0f / 60.0f); - } else if (_rpm_data[i].last_update_us > 0) { + } else if (was_rpm_data_ever_reported(_rpm_data[i])) { // if we have ever received data on an ESC, mark it as valid but with no data // this prevents large frequency shifts when ESCs disappear freqs[valid_escs++] = 0.0f; @@ -103,12 +103,12 @@ uint32_t AP_ESC_Telem::get_active_esc_mask() const { const uint32_t now = AP_HAL::millis(); uint32_t now_us = AP_HAL::micros(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { - if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS - && now_us - _rpm_data[i].last_update_us >= ESC_RPM_DATA_TIMEOUT_US) { + if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) { + // have never seen telem from this ESC continue; } - if (_telem_data[i].last_update_ms == 0 && _rpm_data[i].last_update_us == 0) { - // have never seen telem from this ESC + if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS + && rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) { continue; } ret |= (1U << i); @@ -131,7 +131,7 @@ bool AP_ESC_Telem::are_motors_running(uint32_t servo_channel_mask, float min_rpm if (BIT_IS_SET(servo_channel_mask, i)) { const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[i]; // we choose a relatively strict measure of health so that failsafe actions can rely on the results - if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_CHECK_TIMEOUT_US) { + if (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_CHECK_TIMEOUT_US)) { return false; } if (rpmdata.rpm < min_rpm) { @@ -151,7 +151,7 @@ bool AP_ESC_Telem::is_telemetry_active(uint32_t servo_channel_mask) const for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { if (BIT_IS_SET(servo_channel_mask, i)) { // no data received - if (get_last_telem_data_ms(i) == 0 && _rpm_data[i].last_update_us == 0) { + if (get_last_telem_data_ms(i) == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) { return false; } } @@ -173,8 +173,7 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const } const uint32_t now = AP_HAL::micros(); - if (rpmdata.last_update_us > 0 && (now >= rpmdata.last_update_us) - && (now - rpmdata.last_update_us < ESC_RPM_DATA_TIMEOUT_US)) { + if (rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) { const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f)); rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew); @@ -200,7 +199,7 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const const uint32_t now = AP_HAL::micros(); - if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) { + if (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) { return false; } @@ -305,8 +304,8 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) return; } - uint32_t now = AP_HAL::millis(); - uint32_t now_us = AP_HAL::micros(); + const uint32_t now = AP_HAL::millis(); + const uint32_t now_us = AP_HAL::micros(); // loop through groups of 4 ESCs const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); @@ -326,7 +325,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) const uint8_t esc_id = (i * 4 + j) + esc_offset; if (esc_id < ESC_TELEM_MAX_ESCS && (now - _telem_data[esc_id].last_update_ms <= ESC_TELEM_DATA_TIMEOUT_MS || - now_us - _rpm_data[esc_id].last_update_us <= ESC_RPM_DATA_TIMEOUT_US)) { + rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) { all_stale = false; break; } @@ -469,17 +468,16 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons _have_data = true; - const uint32_t now = AP_HAL::micros(); + const uint32_t now = MAX(1U ,AP_HAL::micros()); // don't allow a value of 0 in, as we use this as a flag in places volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; const auto last_update_us = rpmdata.last_update_us; rpmdata.prev_rpm = rpmdata.rpm; rpmdata.rpm = new_rpm; - if (now > last_update_us) { // cope with wrapping - rpmdata.update_rate_hz = 1.0e6f / (now - last_update_us); - } + rpmdata.update_rate_hz = 1.0e6f / constrain_uint32((now - last_update_us), 100, 1000000U*10U); // limit the update rate 0.1Hz to 10KHz rpmdata.last_update_us = now; rpmdata.error_rate = error_rate; + rpmdata.data_valid = true; #ifdef ESC_TELEM_DEBUG hal.console->printf("RPM: rate=%.1fhz, rpm=%f)\n", rpmdata.update_rate_hz, new_rpm); @@ -490,10 +488,11 @@ void AP_ESC_Telem::update() { AP_Logger *logger = AP_Logger::get_singleton(); - // Push received telemetry data into the logging system - if (logger && logger->logging_enabled()) { + const uint32_t now_us = AP_HAL::micros(); - for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + // Push received telemetry data into the logging system + if (logger && logger->logging_enabled()) { if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { @@ -529,9 +528,32 @@ void AP_ESC_Telem::update() _last_rpm_log_us[i] = _rpm_data[i].last_update_us; } } + + if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { + _rpm_data[i].data_valid = false; + } } } +bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) +{ + // easy case, has the time window been crossed so it's invalid + if ((now_us - instance.last_update_us) > timeout_us) { + return false; + } + // we never got a valid data, to it's invalid + if (instance.last_update_us == 0) { + return false; + } + // check if things generally expired on us, this is done to handle time wrapping + return instance.data_valid; +} + +bool AP_ESC_Telem::was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance) +{ + return instance.last_update_us > 0; +} + #if AP_SCRIPTING_ENABLED /* set RPM scale factor from script diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index fa834b29d1..bf62542da8 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -109,6 +109,10 @@ private: // callback to update the data in the frontend, should be called by the driver when new data is available void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask); + // helper that validates RPM data + static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us); + static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance); + // rpm data volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS]; // telemetry data diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 1ee76c7e3e..e40c09b64a 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -25,8 +25,9 @@ public: float rpm; // rpm float prev_rpm; // previous rpm float error_rate; // error rate in percent - uint32_t last_update_us; // last update time, determines whether active + uint32_t last_update_us; // last update time, greater then 0 means we've gotten data at some point float update_rate_hz; + bool data_valid; // if this isn't set to true, then the ESC data should be ignored }; enum TelemetryType {