fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 03:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag_cal: fix mag bias estimate to mag cal

Browse Source 
- since last_us is set to 0 every time the bias is not observable, the
  total time was also reset -> needed 30 consecutive seconds in mag 3D
  to be declared "stable"
- after landing, the mag_aligned_in_flight flag is reset. Using this for
  bias validity makes it invalid before we have a chance to save it to
  the calibration.
This commit is contained in:
bresch
2023-07-06 14:55:58 +02:00
committed by Daniel Agar
parent 69aa8689c0
commit 2f52926972
3 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+4 -3
View File
@@ -2456,8 +2456,10 @@ void EKF2::UpdateCalibration(const hrt_abstime &timestamp, InFlightCalibration &
// consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit
const float bias_change_limit = 0.1f * bias_limit;
if ((cal.last_us != 0) && !(cal.bias - bias).longerThan(bias_change_limit)) {
cal.total_time_us += timestamp - cal.last_us;
if (!(cal.bias - bias).longerThan(bias_change_limit)) {
if (cal.last_us != 0) {
cal.total_time_us += timestamp - cal.last_us;
}
if (cal.total_time_us > 30_s) {
cal.cal_available = true;
@@ -2517,7 +2519,6 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
{
const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
&& _ekf.control_status_flags().mag_aligned_in_flight
&& !_ekf.control_status_flags().mag_fault
&& !_ekf.control_status_flags().mag_field_disturbed;