mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-03 06:28:32 -04:00
AP_NavEKF3: remove getFilterGpsStatus
Not needed after we moved logging into NavEKF3
This commit is contained in:
parent
aaae7bda6b
commit
dd3ab29b2f
@ -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
|
||||||
{
|
{
|
||||||
|
@ -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
|
||||||
|
@ -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));
|
||||||
|
@ -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
|
||||||
{
|
{
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user