Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: rearrange readGPSData to remove some nesting

Browse Source
This commit is contained in:
Peter Barker 2020-11-27 18:34:36 +11:00 committed by Peter Barker
parent 31ebcdff75
commit 52b8b95a72
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -539,11 +539,20 @@ bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
void NavEKF3_core::readGpsData()
{
// check for new GPS data
// limit update rate to avoid overflowing the FIFO buffer
const auto &gps = dal.gps();
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
// limit update rate to avoid overflowing the FIFO buffer
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
return;
}
if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
return;
}
// report GPS fix status
gpsCheckStatus.bad_fix = false;
@ -690,13 +699,6 @@ void NavEKF3_core::readGpsData()
yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
}
} else {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
}
}
}
// read the delta angle and corresponding time interval from the IMU