diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index d205badff5..fb4b331e6e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1758,19 +1758,6 @@ void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status) const } } -/* -return filter gps quality check status -*/ -void NavEKF3::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - core[instance].getFilterGpsStatus(status); - } else { - memset(&status, 0, sizeof(status)); - } -} - // send an EKF_STATUS_REPORT message to GCS void NavEKF3::send_status_report(mavlink_channel_t chan) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 6e93674780..8670cdce4e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -290,12 +290,6 @@ public: */ void getFilterFaults(int8_t instance, uint16_t &faults) const; - /* - return filter gps quality check status for the specified instance - An out of range instance (eg -1) returns data for the primary instance - */ - void getFilterGpsStatus(int8_t instance, nav_gps_status &faults) const; - /* return filter status flags for the specified instance An out of range instance (eg -1) returns data for the primary instance diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 38e83202fb..d1a7e83d65 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -146,12 +146,10 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const tasTimeout<<4; nav_filter_status solutionStatus {}; - nav_gps_status gpsStatus {}; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); getFilterFaults(_faultStatus); getFilterStatus(solutionStatus); - getFilterGpsStatus(gpsStatus); const struct log_NKF4 pkt4{ LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG), time_us : time_us, @@ -167,7 +165,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const faults : _faultStatus, timeouts : timeoutStatus, solution : solutionStatus.value, - gps : gpsStatus.value, + gps : gpsCheckStatus.value, primary : frontend->getPrimaryCoreIndex() }; AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 1f6bfb8f69..2118c01632 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -526,27 +526,6 @@ void NavEKF3_core::getFilterStatus(nav_filter_status &status) const status = filterStatus; } -/* -return filter gps quality check status -*/ -void NavEKF3_core::getFilterGpsStatus(nav_gps_status &faults) const -{ - // init return value - faults.value = 0; - - // set individual flags - faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient - faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient - faults.flags.bad_vAcc = gpsCheckStatus.bad_vAcc; // reported vertical position accuracy is insufficient - faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use - faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient - faults.flags.bad_horiz_drift = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static) - faults.flags.bad_hdop = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS - faults.flags.bad_vert_vel = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static) - faults.flags.bad_fix = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required - faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) -} - // send an EKF_STATUS message to GCS void NavEKF3_core::send_status_report(mavlink_channel_t chan) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 779e961d6f..93586b1392 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -310,11 +310,6 @@ public: */ void getFilterFaults(uint16_t &faults) const; - /* - return filter gps quality check status - */ - void getFilterGpsStatus(nav_gps_status &status) const; - /* Return a filter function status that indicates: Which outputs are valid @@ -1348,18 +1343,21 @@ private: } faultStatus; // flags indicating which GPS quality checks are failing - struct { - bool bad_sAcc:1; - bool bad_hAcc:1; - bool bad_vAcc:1; - bool bad_yaw:1; - bool bad_sats:1; - bool bad_VZ:1; - bool bad_horiz_drift:1; - bool bad_hdop:1; - bool bad_vert_vel:1; - bool bad_fix:1; - bool bad_horiz_vel:1; + union { + struct { + bool bad_sAcc:1; + bool bad_hAcc:1; + bool bad_vAcc:1; + bool bad_yaw:1; + bool bad_sats:1; + bool bad_VZ:1; + bool bad_horiz_drift:1; + bool bad_hdop:1; + bool bad_vert_vel:1; + bool bad_fix:1; + bool bad_horiz_vel:1; + }; + uint16_t value; } gpsCheckStatus; // states held by magnetometer fusion across time steps