dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/mtecs_estimator' into navigator_rewrite_estimator

This commit is contained in:
Thomas Gubler 2014-06-10 22:50:30 +02:00
parent 9ba89e0cb2 2219dd3fc6
commit a89ed8eed9
2 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf_att_pos_estimator/estimator_21states.cpp
View File

@ -61,8 +61,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
// Apply corrections for earths rotation rate and coning errors
// * and + operators have been overloaded
correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
correctedDelAng.z = 0;
correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
// Convert the rotation vector to its equivalent quaternion
rotationMag = correctedDelAng.length();
if (rotationMag < 1e-12f)

2
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -2266,7 +2266,7 @@ int AttPosEKF::CheckAndBound()
// Reset the filter if gyro offsets are excessive
if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) {
FillErrorReport(&last_ekf_error);
InitializeDynamic(velNED, magDeclination);
// that's all we can do here, return