Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Improve reporting of normalised ekf2 magnetometer innovations

Browse Source 
Report the maximum of the x,y,z axis
This commit is contained in:
Paul Riseborough 2016-05-16 16:45:27 +10:00 committed by Andrew Tridgell
parent a3b78e6231
commit 54213ad871
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/DataFlash/LogFile.cpp
View File

@ -1381,7 +1381,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
ahrs.get_NavEKF2().getVariances(0,velVar, posVar, hgtVar, magVar, tasVar, offset);
float magLength = magVar.length();
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
ahrs.get_NavEKF2().getFilterFaults(0,faultStatus);
ahrs.get_NavEKF2().getFilterTimeouts(0,timeoutStatus);
ahrs.get_NavEKF2().getFilterStatus(0,solutionStatus);
@ -1395,7 +1395,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*magLength),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError,
offsetNorth : (int8_t)(offset.x),
@ -1509,7 +1509,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
// Write 9th EKF packet
ahrs.get_NavEKF2().getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset);
magLength = magVar.length();
tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
ahrs.get_NavEKF2().getFilterFaults(1,faultStatus);
ahrs.get_NavEKF2().getFilterTimeouts(1,timeoutStatus);
ahrs.get_NavEKF2().getFilterStatus(1,solutionStatus);
@ -1521,7 +1521,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*magLength),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError,
offsetNorth : (int8_t)(offset.x),