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

Filter: protect against extremely low notch filter frequencies #25352

Merged
merged 2 commits into from
Nov 2, 2023
Merged

Filter: protect against extremely low notch filter frequencies #25352

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
32133b3
Filter: protect against extremely low notch filter frequencies
tridge Oct 24, 2023
a4c3f67
AP_ESC_Telem: added stale() method for ESC telem
tridge Oct 25, 2023
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
16 changes: 8 additions & 8 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 @@ -107,7 +107,7 @@ uint32_t AP_ESC_Telem::get_active_esc_mask() const {
// have never seen telem from this ESC
continue;
}
if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS
if (_telem_data[i].stale(now)
&& !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
continue;
}
Expand Down Expand Up @@ -211,7 +211,7 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const
bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) {
return false;
}
Expand All @@ -223,7 +223,7 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) {
return false;
}
Expand Down Expand Up @@ -251,7 +251,7 @@ bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const
bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) {
return false;
}
Expand All @@ -263,7 +263,7 @@ bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) {
return false;
}
Expand All @@ -275,7 +275,7 @@ bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) {
return false;
}
Expand All @@ -287,7 +287,7 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah
bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| _telem_data[esc_index].stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) {
return false;
}
Expand Down Expand Up @@ -324,7 +324,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 &&
(now - _telem_data[esc_id].last_update_ms <= ESC_TELEM_DATA_TIMEOUT_MS ||
(!_telem_data[esc_id].stale(now) ||
rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) {
all_stale = false;
break;
Expand Down
13 changes: 12 additions & 1 deletion 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 @@ -40,4 +40,15 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele
_frontend->update_telem_data(esc_index, new_data, data_present_mask);
}

#endif
/*
return true if the data is stale
*/
bool AP_ESC_Telem_Backend::TelemetryData::stale(uint32_t now_ms) const volatile
{
if (now_ms == 0) {
now_ms = AP_HAL::millis();
}
return last_update_ms == 0 || now_ms - last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS;
}

#endif
3 changes: 3 additions & 0 deletions 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 @@ -19,6 +19,9 @@ class AP_ESC_Telem_Backend {
uint32_t last_update_ms; // last update time in milliseconds, determines whether active
uint16_t types; // telemetry types present
uint16_t count; // number of times updated

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

struct RpmData {
Expand Down
2 changes: 1 addition & 1 deletion libraries/Filter/NotchFilter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
_center_freq_hz * NOTCH_MAX_SLEW_UPPER);
}

if ((new_center_freq > 0.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz;
float alpha = sinf(omega) / (2 * Q);
b0 = 1.0 + alpha*sq(A);
Expand Down