diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 8716e49f7f..f7eed9ff85 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -563,5 +563,6 @@ void NavEKF3_core::updateFilterStatus(void) filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_quality_good = gpsGoodToAlign; + filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index a4798e71f1..626f1fd8b7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -140,7 +140,7 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const offsetEast : (int8_t)(offset.y), faults : (uint16_t)(faultStatus), timeouts : (uint8_t)(timeoutStatus), - solution : (uint16_t)(solutionStatus.value), + solution : (uint32_t)(solutionStatus.value), gps : (uint16_t)(gpsStatus.value), primary : (int8_t)primaryIndex }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index ab8abbdb7d..8883b80bcb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -585,6 +585,9 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } + if (!filterStatus.flags.initalized) { + flags |= EKF_UNINITIALIZED; + } if (filterStatus.flags.gps_glitching) { flags |= (1<<15); }