mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-24 00:33:56 -04:00
AP_AHRS : Prevent EKF starting if GPS sats less than AHRS_GPS_MINSATS
This commit is contained in:
parent
9c3379ff48
commit
62339c217c
@ -58,8 +58,8 @@ void AP_AHRS_NavEKF::update(void)
|
|||||||
_dcm_attitude(roll, pitch, yaw);
|
_dcm_attitude(roll, pitch, yaw);
|
||||||
|
|
||||||
if (!ekf_started) {
|
if (!ekf_started) {
|
||||||
// if we have a GPS lock we can start the EKF
|
// if we have a GPS lock and more than 6 satellites, we can start the EKF
|
||||||
if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && get_gps().num_sats() >= _gps_minsats) {
|
||||||
if (start_time_ms == 0) {
|
if (start_time_ms == 0) {
|
||||||
start_time_ms = hal.scheduler->millis();
|
start_time_ms = hal.scheduler->millis();
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user