aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: ekf check adds position variance check

Browse Source 
two of three of compass, velocity and position variances being high will trigger the ekf failsafe
This commit is contained in:
Randy Mackay 2017-07-07 17:17:40 +09:00
parent d64b2fcbf0
commit 2e56e10d9c
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/ekf_check.cpp
View File

@ -100,16 +100,24 @@ bool Copter::ekf_over_threshold()
} }
// use EKF to get variance // use EKF to get variance
float posVar, hgtVar, tasVar; float position_variance, vel_variance, height_variance, tas_variance;
Vector3f magVar; Vector3f mag_variance;
Vector2f offset; Vector2f offset;
float compass_variance; ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
float vel_variance;
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
// return true if compass and velocity variance over the threshold // return true if two of compass, velocity and position variances are over the threshold
return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh); uint8_t over_thresh_count = 0;
if (mag_variance.length() >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (vel_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (position_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
return (over_thresh_count >= 2);
} }