aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: allow initialisation before GPS lock to aid indoor testing

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-18 11:57:59 +11:00 committed by Andrew Tridgell
parent 7fb60812c2
commit b4171853b1
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -52,8 +52,8 @@ void AP_AHRS_NavEKF::update(void)
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// if we have GPS lock and a compass set we can start the EKF
if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
// if we have a compass set we can start the EKF
if (get_compass()) {
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -383,9 +383,9 @@ void NavEKF::UpdateFilter()
void NavEKF::SelectVelPosFusion()
{
// Command fusion of GPS measurements if new ones available
// Command fusion of GPS measurements if new ones available or in static mode
readGpsData();
if (newDataGps) {
if (newDataGps||staticMode) {
fuseVelData = true;
fusePosData = true;
// Calculate the scale factor to be applied to the measurement variance to account for
@ -398,9 +398,9 @@ void NavEKF::SelectVelPosFusion()
fuseVelData = false;
fusePosData = false;
}
// Command fusion of height measurements if new ones available
// Command fusion of height measurements if new ones available or in static mode
readHgtData();
if (newDataHgt)
if (newDataHgt||staticMode)
{
fuseHgtData = true;
// Calculate the scale factor to be applied to the measurement variance to account for
@ -1337,6 +1337,10 @@ void NavEKF::FuseVelPosNED()
{
posHealth = true;
posFailTime = hal.scheduler->millis();
if (posTimeout)
{
ResetPosition();
}
}
else
{
@ -1360,6 +1364,10 @@ void NavEKF::FuseVelPosNED()
{
hgtHealth = true;
hgtFailTime = hal.scheduler->millis();
if (hgtTimeout)
{
ResetPosition();
}
}
else
{