Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: add initalized flag and change to uint32_t

This commit is contained in:
Peter Hall 2020-02-07 20:33:52 +00:00 committed by Andrew Tridgell
parent e0eeb4f863
commit 47e3fda8e9
3 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -563,5 +563,6 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); 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_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign; filterStatus.flags.gps_quality_good = gpsGoodToAlign;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
} }

2
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -140,7 +140,7 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const
offsetEast : (int8_t)(offset.y), offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus), faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus), timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value), solution : (uint32_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value), gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex primary : (int8_t)primaryIndex
}; };

3
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -585,6 +585,9 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
if (filterStatus.flags.pred_horiz_pos_abs) { if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS; flags |= EKF_PRED_POS_HORIZ_ABS;
} }
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
if (filterStatus.flags.gps_glitching) { if (filterStatus.flags.gps_glitching) {
flags |= (1<<15); flags |= (1<<15);
} }