From d7c72af849e2e618f1f9b19da10c3649ac60a9f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Jul 2021 16:16:08 +1000 Subject: [PATCH] AP_GPS: added arming checks for yaw available on ublox RTK rover --- libraries/AP_GPS/AP_GPS.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c35597f099..0b103270f7 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1874,15 +1874,21 @@ bool AP_GPS::prepare_for_arming(void) { } bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { -#if HAL_ENABLE_LIBUAVCAN_DRIVERS for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { +#if HAL_ENABLE_LIBUAVCAN_DRIVERS if (_type[i] == GPS_TYPE_UAVCAN) { if (!AP_GPS_UAVCAN::backends_healthy(failure_msg, failure_msg_len)) { return false; } } - } #endif + if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) { + if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) { + hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1)); + return false; + } + } + } return true; }