Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: rework ekf_check to use the EKF vibration_affected status

Browse Source
This commit is contained in:
Paul Riseborough 2021-07-16 16:40:23 +10:00 committed by Andrew Tridgell
parent ca00da81cd
commit 00806ac9fb
1 changed files with 47 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
ArduCopter/ekf_check.cpp
View File

@ -242,7 +242,7 @@ void Copter::check_vibration()
uint32_t now = AP_HAL::millis();
// assume checks will succeed
bool checks_succeeded = true;
bool innovation_checks_valid = true;
// check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
Vector3f vel_innovation;
@ -251,7 +251,7 @@ void Copter::check_vibration()
float tas_innovation;
float yaw_innovation;
if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
checks_succeeded = false;
innovation_checks_valid = false;
}
const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
@ -259,46 +259,59 @@ void Copter::check_vibration()
float position_variance, vel_variance, height_variance, tas_variance;
Vector3f mag_variance;
if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) {
checks_succeeded = false;
innovation_checks_valid = false;
}
// if no failure
if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || (vel_variance < 1.0f)) {
if (vibration_check.high_vibes) {
// start clear time
if (vibration_check.clear_ms == 0) {
vibration_check.clear_ms = now;
return;
}
// turn off vibration compensation after 15 seconds
if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user
vibration_check.high_vibes = false;
pos_control->set_vibe_comp(false);
vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
}
const bool is_vibration_affected = ahrs.is_vibration_affected();
const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance > 1.0f)) || is_vibration_affected;
const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed();
// static uint32_t frame_count=0;
// frame_count++;
// if (frame_count > 10) {
// frame_count=0;
// AP::logger().Write(
// "CEVC",
// "TimeUS,icv,ivpp,vv,iva,dbva",
// "Qbbfbb",
// AP_HAL::micros64(),
// innovation_checks_valid,
// innov_velD_posD_positive,
// vel_variance,
// is_vibration_affected,
// do_bad_vibe_actions);
// }
if (!vibration_check.high_vibes) {
// initialise timers
if (!do_bad_vibe_actions) {
vibration_check.start_ms = now;
}
vibration_check.start_ms = 0;
return;
}
// start timer
if (vibration_check.start_ms == 0) {
vibration_check.start_ms = now;
vibration_check.clear_ms = 0;
return;
}
// check if failure has persisted for at least 1 second
if (now - vibration_check.start_ms > 1000) {
if (!vibration_check.high_vibes) {
// check if failure has persisted for at least 1 second
if (now - vibration_check.start_ms > 1000) {
// switch ekf to use resistant gains
vibration_check.clear_ms = 0;
vibration_check.high_vibes = true;
pos_control->set_vibe_comp(true);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
}
} else {
// initialise timer
if (do_bad_vibe_actions) {
vibration_check.clear_ms = now;
}
// turn off vibration compensation after 15 seconds
if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user
vibration_check.start_ms = 0;
vibration_check.high_vibes = false;
pos_control->set_vibe_comp(false);
vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
}
}
return;
}