Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ESC_Telem: Fix some time wrap issues that can lead to using stale data if a ESC stops responding

This commit is contained in:
Michael du Breuil 2023-07-21 10:07:13 -07:00 committed by Andrew Tridgell
parent c1e6f130dd
commit 935fad54ad
3 changed files with 48 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -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++) {
// 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,7 +528,30 @@ 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

4
libraries/AP_ESC_Telem/AP_ESC_Telem.h
View File

@ -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

3
libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View File

@ -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 {