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

GPS reinit completed, put in NaN catcher

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/fw_att_pos_estimator/estimator.h
View File

@ -122,8 +122,8 @@ extern float baroHgt;
extern bool statesInitialised;
const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
void UpdateStrapdownEquationsNED();

20
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -694,13 +694,20 @@ FixedwingEstimator::task_main()
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
InitialiseFilter(velNED);
warnx("GPS REINIT");
_gps_initialized = true;
} else if (!statesInitialised) {
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
velNED[0] = 0.0f;
velNED[1] = 0.0f;
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);
}
}
@ -970,7 +977,14 @@ FixedwingEstimator::task_main()
/* advertise and publish */
_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.x = states[7];
_local_pos.y = states[8];