AP_NavEKF3: remove getFilterGpsStatus

Not needed after we moved logging into NavEKF3
This commit is contained in:
Peter Barker 2021-05-30 12:24:16 +10:00 committed by Andrew Tridgell
parent aaae7bda6b
commit dd3ab29b2f
5 changed files with 16 additions and 60 deletions

View File

@ -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 // send an EKF_STATUS_REPORT message to GCS
void NavEKF3::send_status_report(mavlink_channel_t chan) const void NavEKF3::send_status_report(mavlink_channel_t chan) const
{ {

View File

@ -290,12 +290,6 @@ public:
*/ */
void getFilterFaults(int8_t instance, uint16_t &faults) const; 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 return filter status flags for the specified instance
An out of range instance (eg -1) returns data for the primary instance An out of range instance (eg -1) returns data for the primary instance

View File

@ -146,12 +146,10 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
tasTimeout<<4; tasTimeout<<4;
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
getFilterFaults(_faultStatus); getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus); getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus);
const struct log_NKF4 pkt4{ const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG),
time_us : time_us, time_us : time_us,
@ -167,7 +165,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
faults : _faultStatus, faults : _faultStatus,
timeouts : timeoutStatus, timeouts : timeoutStatus,
solution : solutionStatus.value, solution : solutionStatus.value,
gps : gpsStatus.value, gps : gpsCheckStatus.value,
primary : frontend->getPrimaryCoreIndex() primary : frontend->getPrimaryCoreIndex()
}; };
AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); AP::logger().WriteBlock(&pkt4, sizeof(pkt4));

View File

@ -526,27 +526,6 @@ void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
status = filterStatus; 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 // send an EKF_STATUS message to GCS
void NavEKF3_core::send_status_report(mavlink_channel_t chan) const void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
{ {

View File

@ -310,11 +310,6 @@ public:
*/ */
void getFilterFaults(uint16_t &faults) const; 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: Return a filter function status that indicates:
Which outputs are valid Which outputs are valid
@ -1348,6 +1343,7 @@ private:
} faultStatus; } faultStatus;
// flags indicating which GPS quality checks are failing // flags indicating which GPS quality checks are failing
union {
struct { struct {
bool bad_sAcc:1; bool bad_sAcc:1;
bool bad_hAcc:1; bool bad_hAcc:1;
@ -1360,6 +1356,8 @@ private:
bool bad_vert_vel:1; bool bad_vert_vel:1;
bool bad_fix:1; bool bad_fix:1;
bool bad_horiz_vel:1; bool bad_horiz_vel:1;
};
uint16_t value;
} gpsCheckStatus; } gpsCheckStatus;
// states held by magnetometer fusion across time steps // states held by magnetometer fusion across time steps