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