Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_ESC_Telem: fix other timeout race #29028

Draft
wants to merge 3 commits into
base: master
Choose a base branch
from
+36 −29
Draft

AP_ESC_Telem: fix other timeout race #29028

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8ab9ff9
AP_ESC_Telem: fix RPM timeout race
robertlong13 Jan 3, 2025
6d50a36
AP_Vehicle: add comment for ESC Telem update rate
robertlong13 Jan 3, 2025
c81cb69
AP_ESC_Telem: fix timeout race
robertlong13 Jan 9, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 26 additions & 24 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,15 +104,12 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con
// ESC_TELEM_DATA_TIMEOUT_MS/ESC_RPM_DATA_TIMEOUT_US
uint32_t AP_ESC_Telem::get_active_esc_mask() const {
uint32_t ret = 0;
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 (_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].stale(now)
&& !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
if (_telem_data[i].stale() && !_rpm_data[i].data_valid) {
continue;
}
ret |= (1U << i);
Expand All @@ -125,15 +122,12 @@ uint8_t AP_ESC_Telem::get_max_rpm_esc() const
{
uint32_t ret = 0;
float max_rpm = 0;
const uint32_t now = AP_HAL::millis();
const uint32_t now_us = AP_HAL::micros();
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
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].stale(now)
&& !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
if (_telem_data[i].stale() && !_rpm_data[i].data_valid) {
continue;
}
if (_rpm_data[i].rpm > max_rpm) {
Expand All @@ -153,13 +147,12 @@ uint8_t AP_ESC_Telem::get_num_active_escs() const {
// return the whether all the motors in servo_channel_mask are running
bool AP_ESC_Telem::are_motors_running(uint32_t servo_channel_mask, float min_rpm, float max_rpm) const
{
const uint32_t now = AP_HAL::micros();

for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
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 (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_CHECK_TIMEOUT_US)) {
if (!rpm_data_within_timeout(rpmdata, ESC_RPM_CHECK_TIMEOUT_US)) {
return false;
}
if (rpmdata.rpm < min_rpm) {
Expand Down Expand Up @@ -201,7 +194,7 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
}

const uint32_t now = AP_HAL::micros();
if (rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) {
if (rpmdata.data_valid) {
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);

Expand All @@ -225,9 +218,7 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const

const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];

const uint32_t now = AP_HAL::micros();

if (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) {
if (!rpmdata.data_valid) {
return false;
}

Expand Down Expand Up @@ -412,9 +403,6 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
return;
}

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);

Expand All @@ -435,8 +423,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
for (uint8_t j=0; j<4; j++) {
const uint8_t esc_id = (i * 4 + j) + esc_offset;
if (esc_id < ESC_TELEM_MAX_ESCS &&
(!_telem_data[esc_id].stale(now) ||
rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) {
(!_telem_data[esc_id].stale() || _rpm_data[esc_id].data_valid)) {
all_stale = false;
break;
}
Expand Down Expand Up @@ -593,6 +580,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
telemdata.count++;
telemdata.types |= data_mask;
telemdata.last_update_ms = AP_HAL::millis();
telemdata.any_data_valid = true;
}

// record an update to the RPM together with timestamp, this allows the notch values to be slewed
Expand Down Expand Up @@ -808,23 +796,37 @@ void AP_ESC_Telem::update()
}
#endif // HAL_LOGGING_ENABLED

const uint32_t now_us = AP_HAL::micros();
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
// copy the last_updated_us timestamp to avoid any race issues
const uint32_t last_updated_us = _rpm_data[i].last_update_us;
const uint32_t now_us = AP_HAL::micros();
// Invalidate RPM data if not received for too long
if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) {
if (AP_HAL::timeout_expired(last_updated_us, now_us, ESC_RPM_DATA_TIMEOUT_US)) {
_rpm_data[i].data_valid = false;
}
const uint32_t last_telem_data_ms = _telem_data[i].last_update_ms;
const uint32_t now_ms = AP_HAL::millis();
// Invalidate telemetry data if not received for too long
if (AP_HAL::timeout_expired(last_telem_data_ms, now_ms, ESC_TELEM_DATA_TIMEOUT_MS)) {
_telem_data[i].any_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)
// NOTE: This function should only be used to check timeouts other than
// ESC_RPM_DATA_TIMEOUT_US. Timeouts equal to ESC_RPM_DATA_TIMEOUT_US should
// use RpmData::data_valid, which is cheaper and achieves the same result.
bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t timeout_us)
{
// copy the last_update_us timestamp to avoid any race issues
const uint32_t last_update_us = instance.last_update_us;
const uint32_t now_us = AP_HAL::micros();
// easy case, has the time window been crossed so it's invalid
if ((now_us - instance.last_update_us) > timeout_us) {
if (AP_HAL::timeout_expired(last_update_us, now_us, timeout_us)) {
return false;
}
// we never got a valid data, to it's invalid
if (instance.last_update_us == 0) {
if (last_update_us == 0) {
return false;
}
// check if things generally expired on us, this is done to handle time wrapping
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ class AP_ESC_Telem {
private:

// 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 rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t timeout_us);
static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance);

#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,9 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele
/*
return true if the data is stale
*/
bool AP_ESC_Telem_Backend::TelemetryData::stale(uint32_t now_ms) const volatile
bool AP_ESC_Telem_Backend::TelemetryData::stale() const volatile
{
return last_update_ms == 0 || now_ms - last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS;
return last_update_ms == 0 || !any_data_valid;
}

/*
Expand All @@ -62,7 +62,7 @@ bool AP_ESC_Telem_Backend::TelemetryData::valid(const uint16_t type_mask) const
// Requested type not available
return false;
}
return !stale(AP_HAL::millis());
return !stale();
}

#endif
5 changes: 4 additions & 1 deletion libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,11 @@ class AP_ESC_Telem_Backend {
uint8_t power_percentage; // Percentage of output power
#endif // AP_EXTENDED_ESC_TELEM_ENABLED

// set to false if no data has been received within the timeout period
bool any_data_valid;

// return true if the data is stale
bool stale(uint32_t now_ms) const volatile;
bool stale() const volatile;

// return true if the requested type of data is available and not stale
bool valid(const uint16_t type_mask) const volatile;
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Vehicle/AP_Vehicle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -624,6 +624,8 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
#endif
SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20, 225),
#if HAL_WITH_ESC_TELEM
// This update function is responsible for checking timeouts and invalidating the ESC telemetry data.
// Be mindful of this if you are planning to reduce the frequency from 100Hz.
SCHED_TASK_CLASS(AP_ESC_Telem, &vehicle.esc_telem, update, 100, 50, 230),
#endif
#if AP_SERVO_TELEM_ENABLED
Expand Down
Loading