Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replay: Add EKF fault status logging message to EKF4

Browse Source
This commit is contained in:
priseborough 2014-05-14 18:39:18 +10:00 committed by Andrew Tridgell
parent f01cc78d37
commit deb16ac5bb
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/Replay/Replay.pde
View File

@ -229,7 +229,7 @@ void setup()
fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n");
fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n"); fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n");
fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE\n"); fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n");
ahrs.set_ekf_use(true); ahrs.set_ekf_use(true);
@ -358,6 +358,8 @@ void loop()
Vector3f magVar; Vector3f magVar;
float tasVar; float tasVar;
Vector2f offset; Vector2f offset;
uint8_t faultStatus;
float deltaGyroBias;
const Matrix3f &dcm_matrix = ((AP_AHRS_DCM)ahrs).get_dcm_matrix(); const Matrix3f &dcm_matrix = ((AP_AHRS_DCM)ahrs).get_dcm_matrix();
dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z);
@ -371,6 +373,7 @@ void loop()
NavEKF.getMagXYZ(magXYZ); NavEKF.getMagXYZ(magXYZ);
NavEKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); NavEKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
NavEKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); NavEKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
NavEKF.getFilterFaults(faultStatus,deltaGyroBias);
NavEKF.getPosNED(ekf_relpos); NavEKF.getPosNED(ekf_relpos);
Vector3f inav_pos = inertial_nav.get_position() * 0.01f; Vector3f inav_pos = inertial_nav.get_position() * 0.01f;
float temp = degrees(ekf_euler.z); float temp = degrees(ekf_euler.z);
@ -523,9 +526,10 @@ void loop()
int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX));
int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX));
int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX));
uint8_t divergeRate = (uint8_t)(100*deltaGyroBias);
// print EKF4 data packet // print EKF4 data packet
fprintf(ekf4f, "%.3f %d %d %d %d %d %d %d %d %d %d\n", fprintf(ekf4f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n",
hal.scheduler->millis() * 0.001f, hal.scheduler->millis() * 0.001f,
hal.scheduler->millis(), hal.scheduler->millis(),
sqrtvarV, sqrtvarV,
@ -536,7 +540,9 @@ void loop()
sqrtvarMZ, sqrtvarMZ,
sqrtvarVT, sqrtvarVT,
offsetNorth, offsetNorth,
offsetEast); offsetEast,
faultStatus,
divergeRate);
} }
} }
} }