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

EKF init improvements

This commit is contained in:
Lorenz Meier 2014-07-13 12:38:43 +02:00
parent 67376c67e6
commit a32577377b
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -610,6 +610,10 @@ FixedwingEstimator::check_filter_state()
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main()
_ekf->FuseVelposNED();
} else if (!_gps_initialized) {
// force static mode
_ekf->staticMode = true;
// Convert GPS measurements to Pos NE, hgt and Vel NED
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;

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

@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
void AttPosEKF::OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
}
@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
current_ekf_state.statesNaN = false;
current_ekf_state.velHealth = true;
//current_ekf_state.posHealth = ?;
//current_ekf_state.hgtHealth = ?;
current_ekf_state.posHealth = true;
current_ekf_state.hgtHealth = true;
current_ekf_state.velTimeout = false;
//current_ekf_state.posTimeout = ?;
//current_ekf_state.hgtTimeout = ?;
current_ekf_state.posTimeout = false;
current_ekf_state.hgtTimeout = false;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
// Fill variables with valid data
velNED[0] = initvelNED[0];