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

EKF init improvements

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8
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.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); 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.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.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main()
_ekf->FuseVelposNED(); _ekf->FuseVelposNED();
} else if (!_gps_initialized) { } else if (!_gps_initialized) {
// force static mode
_ekf->staticMode = true;
// Convert GPS measurements to Pos NE, hgt and Vel NED // Convert GPS measurements to Pos NE, hgt and Vel NED
_ekf->velNED[0] = 0.0f; _ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f; _ekf->velNED[1] = 0.0f;
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+11 -5
View File
@@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
void AttPosEKF::OnGroundCheck() 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) { if (staticMode) {
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); 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.statesNaN = false;
current_ekf_state.velHealth = true; current_ekf_state.velHealth = true;
//current_ekf_state.posHealth = ?; current_ekf_state.posHealth = true;
//current_ekf_state.hgtHealth = ?; current_ekf_state.hgtHealth = true;
current_ekf_state.velTimeout = false; current_ekf_state.velTimeout = false;
//current_ekf_state.posTimeout = ?; current_ekf_state.posTimeout = false;
//current_ekf_state.hgtTimeout = ?; current_ekf_state.hgtTimeout = false;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
// Fill variables with valid data // Fill variables with valid data
velNED[0] = initvelNED[0]; velNED[0] = initvelNED[0];