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: vibration compensation turns off in manual modes

Browse Source
This commit is contained in:
Randy Mackay 2022-04-27 15:20:22 +09:00
parent 2a700f677a
commit 1dafa2ba9c
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ekf_check.cpp
View File

@ -262,7 +262,7 @@ void Copter::check_vibration()
const bool is_vibration_affected = ahrs.is_vibration_affected(); 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 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(); const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed() && !flightmode->has_manual_throttle();
if (!vibration_check.high_vibes) { if (!vibration_check.high_vibes) {
// initialise timers // initialise timers
@ -271,7 +271,7 @@ void Copter::check_vibration()
} }
// check if failure has persisted for at least 1 second // check if failure has persisted for at least 1 second
if (now - vibration_check.start_ms > 1000) { if (now - vibration_check.start_ms > 1000) {
// switch ekf to use resistant gains // switch position controller to use resistant gains
vibration_check.clear_ms = 0; 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);
@ -285,7 +285,7 @@ void Copter::check_vibration()
} }
// turn off vibration compensation after 15 seconds // turn off vibration compensation after 15 seconds
if (now - vibration_check.clear_ms > 15000) { if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user // restore position controller gains, reset timers and update user
vibration_check.start_ms = 0; vibration_check.start_ms = 0;
vibration_check.high_vibes = false; vibration_check.high_vibes = false;
pos_control->set_vibe_comp(false); pos_control->set_vibe_comp(false);