diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index c9dfef5775..8245f5a4a5 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -240,6 +240,12 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb) interim_state.num_sats = 0; } + if (!seen_aux) { + // if we haven't seen an Aux message then populate vdop and + // hdop from pdop. Some GPS modules don't provide the Aux message + interim_state.hdop = interim_state.vdop = cb.msg->pdop * 100.0; + } + interim_state.last_gps_time_ms = AP_HAL::millis(); _new_data = true; @@ -345,6 +351,12 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) interim_state.num_sats = 0; } + if (!seen_aux) { + // if we haven't seen an Aux message then populate vdop and + // hdop from pdop. Some GPS modules don't provide the Aux message + interim_state.hdop = interim_state.vdop = cb.msg->pdop * 100.0; + } + interim_state.last_gps_time_ms = AP_HAL::millis(); _new_data = true; @@ -364,10 +376,12 @@ void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb) WITH_SEMAPHORE(sem); if (!uavcan::isNaN(cb.msg->hdop)) { + seen_aux = true; interim_state.hdop = cb.msg->hdop * 100.0; } if (!uavcan::isNaN(cb.msg->vdop)) { + seen_aux = true; interim_state.vdop = cb.msg->vdop * 100.0; } } diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index a666aa674a..a064254815 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -64,6 +64,7 @@ private: uint8_t _detected_module; bool seen_message; bool seen_fix2; + bool seen_aux; // Module Detection Registry static struct DetectedModules {