Browse Source

AP_NavEKF3: remove getFilterTimeouts access methods

c415-sdk
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 @@ -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
*/

14
libraries/AP_NavEKF3/AP_NavEKF3.h

@ -297,20 +297,6 @@ public: @@ -297,20 +297,6 @@ public:
*/
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
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 @@ -138,13 +138,18 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
float tasVar = 0;
uint16_t _faultStatus=0;
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_gps_status gpsStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
getFilterFaults(_faultStatus);
getFilterTimeouts(timeoutStatus);
getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus);
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 @@ -512,26 +512,6 @@ void NavEKF3_core::getFilterFaults(uint16_t &faults) const
!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
void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
{

13
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -317,19 +317,6 @@ public: @@ -317,19 +317,6 @@ public:
*/
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
*/

2
libraries/AP_NavEKF3/LogStructure.h

@ -185,7 +185,7 @@ struct PACKED log_XKF3 { @@ -185,7 +185,7 @@ struct PACKED log_XKF3 {
// @Field: OFN: Most recent position recent magnitude (North component)
// @Field: OFE: Most recent position recent magnitude (East component)
// @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: GPS: Filter GPS status
// @Field: PI: Primary core index

Loading…
Cancel
Save