Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: add getVelInnovationsAndVariancesForSource

Browse Source
This commit is contained in:
Randy Mackay 2020-10-13 14:20:48 +09:00
parent 58aa6086df
commit ba9f60abc7
4 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1407,6 +1407,19 @@ void NavEKF3::getStateVariances(int8_t instance, float stateVar[24]) const
}
}
// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance
// returns true on success and results are placed in innovations and variances arguments
bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
{
if (instance < 0 || instance >= num_cores) {
instance = primary;
}
if (core) {
return core[instance].getVelInnovationsAndVariancesForSource(source, innovations, variances);
}
return false;
}
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool NavEKF3::use_compass(void) const

4
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -198,6 +198,10 @@ public:
// return the diagonals from the covariance matrix for the specified instance
void getStateVariances(int8_t instance, float stateVar[24]) const;
// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance
// returns true on success and results are placed in innovations and variances arguments
bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;

30
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -513,6 +513,36 @@ void NavEKF3_core::getStateVariances(float stateVar[24])
}
}
// get a particular source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments
bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
{
switch (source) {
case AP_NavEKF_Source::SourceXY::GPS:
// check for timeouts
if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
return false;
}
innovations = gpsVelInnov;
variances = gpsVelVarInnov;
return true;
case AP_NavEKF_Source::SourceXY::EXTNAV:
// check for timeouts
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
return false;
}
innovations = extNavVelInnov;
variances = extNavVelVarInnov;
return true;
default:
// variances are not available for this source
return false;
}
// should never get here but just in case
return false;
}
/*
return the filter fault status as a bitmasked integer
0 = quaternions are NaN

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -211,6 +211,10 @@ public:
// return the diagonals from the covariance matrix
void getStateVariances(float stateVar[24]);
// get a particular source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments
bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;