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

GPS reinit completed, put in NaN catcher

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-21 13:44:34 +01:00
parent c5311b18fe
commit 57df53d27c
2 changed files with 19 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/estimator.h
+2 -2
View File
@@ -122,8 +122,8 @@ extern float baroHgt;
extern bool statesInitialised; extern bool statesInitialised;
const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
void UpdateStrapdownEquationsNED(); void UpdateStrapdownEquationsNED();
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+17 -3
View File
@@ -694,13 +694,20 @@ FixedwingEstimator::task_main()
velNED[1] = _gps.vel_e_m_s; velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s; velNED[2] = _gps.vel_d_m_s;
InitialiseFilter(velNED); InitialiseFilter(velNED);
warnx("GPS REINIT");
_gps_initialized = true; _gps_initialized = true;
} else if (!statesInitialised) { } else if (!statesInitialised) {
velNED[0] = _gps.vel_n_m_s; velNED[0] = 0.0f;
velNED[1] = _gps.vel_e_m_s; velNED[1] = 0.0f;
velNED[2] = _gps.vel_d_m_s; velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
InitialiseFilter(velNED); InitialiseFilter(velNED);
} }
} }
@@ -970,7 +977,14 @@ FixedwingEstimator::task_main()
/* advertise and publish */ /* advertise and publish */
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
} }
}
if (!isfinite(states[0])) {
print_status();
_exit(0);
}
if (_gps_initialized) {
_local_pos.timestamp = last_sensor_timestamp; _local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = states[7]; _local_pos.x = states[7];
_local_pos.y = states[8]; _local_pos.y = states[8];