Browse Source

AP_NavEKF3: remove getFilterTimeouts access methods

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
commit
3582b69e9c
  1. 21
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 14
      libraries/AP_NavEKF3/AP_NavEKF3.h
  3. 9
      libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
  4. 20
      libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
  5. 13
      libraries/AP_NavEKF3/AP_NavEKF3_core.h
  6. 2
      libraries/AP_NavEKF3/LogStructure.h

21
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -1749,27 +1749,6 @@ void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const
} }
} }
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
4 = unassigned
5 = unassigned
6 = unassigned
7 = unassigned
*/
void NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getFilterTimeouts(timeouts);
} else {
timeouts = 0;
}
}
/* /*
return filter status flags return filter status flags
*/ */

14
libraries/AP_NavEKF3/AP_NavEKF3.h

@ -297,20 +297,6 @@ public:
*/ */
void getFilterFaults(int8_t instance, uint16_t &faults) const; void getFilterFaults(int8_t instance, uint16_t &faults) const;
/*
return filter timeout status as a bitmasked integer for the specified instance
An out of range instance (eg -1) returns data for the primary instance
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
4 = unassigned
5 = unassigned
6 = unassigned
7 = unassigned
*/
void getFilterTimeouts(int8_t instance, uint8_t &timeouts) const;
/* /*
return filter gps quality check status for the specified instance return filter gps quality check status 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

9
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@ -138,13 +138,18 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
float tasVar = 0; float tasVar = 0;
uint16_t _faultStatus=0; uint16_t _faultStatus=0;
Vector2f offset; Vector2f offset;
uint8_t timeoutStatus=0; const uint8_t timeoutStatus =
posTimeout<<0 |
velTimeout<<1 |
hgtTimeout<<2 |
magTimeout<<3 |
tasTimeout<<4;
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {}; 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);
getFilterTimeouts(timeoutStatus);
getFilterStatus(solutionStatus); getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus); getFilterGpsStatus(gpsStatus);
const struct log_NKF4 pkt4{ const struct log_NKF4 pkt4{

20
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@ -512,26 +512,6 @@ void NavEKF3_core::getFilterFaults(uint16_t &faults) const
!statesInitialised<<7); !statesInitialised<<7);
} }
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
4 = true airspeed measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
*/
void NavEKF3_core::getFilterTimeouts(uint8_t &timeouts) const
{
timeouts = (posTimeout<<0 |
velTimeout<<1 |
hgtTimeout<<2 |
magTimeout<<3 |
tasTimeout<<4);
}
// Return the navigation filter status message // Return the navigation filter status message
void NavEKF3_core::getFilterStatus(nav_filter_status &status) const void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
{ {

13
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -317,19 +317,6 @@ public:
*/ */
void getFilterFaults(uint16_t &faults) const; void getFilterFaults(uint16_t &faults) const;
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
7 = unassigned
*/
void getFilterTimeouts(uint8_t &timeouts) const;
/* /*
return filter gps quality check status return filter gps quality check status
*/ */

2
libraries/AP_NavEKF3/LogStructure.h

@ -185,7 +185,7 @@ struct PACKED log_XKF3 {
// @Field: OFN: Most recent position recent magnitude (North component) // @Field: OFN: Most recent position recent magnitude (North component)
// @Field: OFE: Most recent position recent magnitude (East component) // @Field: OFE: Most recent position recent magnitude (East component)
// @Field: FS: Filter fault status // @Field: FS: Filter fault status
// @Field: TS: Filter timeout status // @Field: TS: Filter timeout status bitmask (0:position measurement, 1:velocity measurement, 2:height measurement, 3:magnetometer measurement, 4:airspeed measurement)
// @Field: SS: Filter solution status // @Field: SS: Filter solution status
// @Field: GPS: Filter GPS status // @Field: GPS: Filter GPS status
// @Field: PI: Primary core index // @Field: PI: Primary core index

Loading…
Cancel
Save