diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 21f4a2738b..66354b47fa 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1661,6 +1661,7 @@ void AP_GPS::calc_blended_state(void) timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0; timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0; + bool data_should_be_logged = false; // combine the states into a blended solution for (uint8_t i=0; i timing[GPS_BLENDED_INSTANCE].last_message_time_ms) { timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms; + data_should_be_logged = true; } } @@ -1801,6 +1803,13 @@ void AP_GPS::calc_blended_state(void) } timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1; timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2; + +#ifndef HAL_BUILD_AP_PERIPH + if (data_should_be_logged && + should_log()) { + AP::logger().Write_GPS(GPS_BLENDED_INSTANCE); + } +#endif } #endif // GPS_BLENDED_INSTANCE