aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: make logging a core concern

Browse Source 
Also dissolve some methods only used for logging
This commit is contained in:
Peter Barker 2020-11-10 22:51:23 +11:00
parent 914629351f
commit e1a033b296
6 changed files with 188 additions and 452 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1239,39 +1239,6 @@ void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const
} }
} }
// return the magnetometer in use for the specified instance
uint8_t NavEKF3::getActiveMag(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveMag();
} else {
return 255;
}
}
// return the baro in use for the specified instance
uint8_t NavEKF3::getActiveBaro(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveBaro();
} else {
return 255;
}
}
// return the GPS in use for the specified instance
uint8_t NavEKF3::getActiveGPS(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveGPS();
} else {
return 255;
}
}
// return the airspeed sensor in use for the specified instance // return the airspeed sensor in use for the specified instance
uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const
{ {
@ -1408,15 +1375,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
} }
} }
// publish output observer angular, velocity and position tracking error
void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getOutputTrackingError(error);
}
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {
@ -1426,15 +1384,6 @@ void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float
} }
} }
// return the diagonals from the covariance matrix for the specified instance
void NavEKF3::getStateVariances(int8_t instance, float stateVar[24]) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getStateVariances(stateVar);
}
}
// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance // 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 // 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 bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
@ -1538,15 +1487,6 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
} }
// return data for debugging optical flow fusion // return data for debugging optical flow fusion
void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov,
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr);
}
}
/* /*
* Write body frame linear and angular displacement measurements from a visual odometry sensor * Write body frame linear and angular displacement measurements from a visual odometry sensor
* *
@ -1603,19 +1543,6 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec
return ret; return ret;
} }
// return data for debugging EKF-GSF yaw estimator
// return false if data not available
bool NavEKF3::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
if (core[instance].getDataEKFGSF(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight)) {
return true;
}
}
return false;
}
// parameter conversion of EKF3 parameters // parameter conversion of EKF3 parameters
void NavEKF3::convert_parameters() void NavEKF3::convert_parameters()
{ {
@ -1717,18 +1644,6 @@ void NavEKF3::convert_parameters()
} }
} }
// return data for debugging range beacon fusion
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow, posNED);
} else {
return false;
}
}
// called by vehicle code to specify that a takeoff is happening // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction // causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
// causes the EKF to start the EKF-GSF yaw estimator // causes the EKF to start the EKF-GSF yaw estimator
@ -2073,21 +1988,6 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
} }
/*
get timing statistics structure
*/
void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) const
{
if (instance < 0 || instance >= num_cores) {
instance = primary;
}
if (core) {
core[instance].getTimingStatistics(timing);
} else {
memset(&timing, 0, sizeof(timing));
}
}
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3::writeDefaultAirSpeed(float airspeed) void NavEKF3::writeDefaultAirSpeed(float airspeed)
{ {

51
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -141,9 +141,6 @@ public:
// return the sensor in use for the specified instance // return the sensor in use 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
uint8_t getActiveMag(int8_t instance) const;
uint8_t getActiveBaro(int8_t instance) const;
uint8_t getActiveGPS(int8_t instance) const;
uint8_t getActiveAirspeed(int8_t instance) const; uint8_t getActiveAirspeed(int8_t instance) const;
// Return estimated magnetometer offsets // Return estimated magnetometer offsets
@ -189,16 +186,10 @@ public:
// 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
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance // return the innovation consistency test ratios 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
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// 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 // 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 // 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; bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
@ -251,27 +242,6 @@ public:
*/ */
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const; uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const;
// return data for debugging optical flow fusion for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
/*
Returns the following data for debugging range beacon fusion
ID : beacon identifier
rng : measured range to beacon (m)
innov : range innovation (m)
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
offsetHigh : high hypothesis for range beacons system vertical offset (m)
offsetLow : low hypothesis for range beacons system vertical offset (m)
posNED : North,East,Down position estimate of receiver from 3-state filter
returns true if data could be found, false if it could not
*/
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED) const;
/* /*
* Writes the measurement from a yaw angle sensor * Writes the measurement from a yaw angle sensor
* *
@ -395,9 +365,6 @@ public:
// allow the enable flag to be set by Replay // allow the enable flag to be set by Replay
void set_enable(bool enable) { _enable.set_enable(enable); } void set_enable(bool enable) { _enable.set_enable(enable); }
// get timing statistics structure
void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const;
/* /*
check if switching lanes will reduce the normalised check if switching lanes will reduce the normalised
innovations. This is called when the vehicle code is about to innovations. This is called when the vehicle code is about to
@ -424,10 +391,6 @@ public:
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed); void writeDefaultAirSpeed(float airspeed);
// log debug data for yaw estimator
// return false if data not available
bool getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const;
// parameter conversion // parameter conversion
void convert_parameters(); void convert_parameters();
@ -611,20 +574,6 @@ private:
// checks for alignment // checks for alignment
bool coreBetterScore(uint8_t new_core, uint8_t current_core) const; bool coreBetterScore(uint8_t new_core, uint8_t current_core) const;
// logging functions shared by cores:
void Log_Write_XKF1(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF2(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF3(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF4(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF5(uint8_t core, uint64_t time_us) const;
void Log_Write_XKFS(uint8_t core, uint64_t time_us) const;
void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
void Log_Write_Beacon(uint8_t core, uint64_t time_us) const;
void Log_Write_BodyOdom(uint8_t core, uint64_t time_us) const;
void Log_Write_State_Variances(uint8_t core, uint64_t time_us) const;
void Log_Write_Timing(uint8_t core, uint64_t time_us) const;
void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
// position, velocity and yaw source control // position, velocity and yaw source control
AP_NavEKF_Source sources; AP_NavEKF_Source sources;
}; };

345
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -1,11 +1,12 @@
#include "AP_NavEKF3.h" #include "AP_NavEKF3.h"
#include "AP_NavEKF3_core.h"
#include <AP_HAL/HAL.h> #include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#include <AP_DAL/AP_DAL.h> #include <AP_DAL/AP_DAL.h>
void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
{ {
// Write first EKF packet // Write first EKF packet
Vector3f euler; Vector3f euler;
@ -15,19 +16,19 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const
Vector3f gyroBias; Vector3f gyroBias;
float posDownDeriv; float posDownDeriv;
Location originLLH; Location originLLH;
getEulerAngles(_core,euler); getEulerAngles(euler);
getVelNED(_core,velNED); getVelNED(velNED);
getPosNE(_core,posNE); getPosNE(posNE);
getPosD(_core,posD); getPosD(posD);
getGyroBias(_core,gyroBias); getGyroBias(gyroBias);
posDownDeriv = getPosDownDerivative(_core); posDownDeriv = getPosDownDerivative();
if (!getOriginLLH(_core,originLLH)) { if (!getOriginLLH(originLLH)) {
originLLH.alt = 0; originLLH.alt = 0;
} }
const struct log_EKF1 pkt{ const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string) pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string) yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
@ -46,21 +47,21 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const
{ {
// Write second EKF packet // Write second EKF packet
Vector3f accelBias; Vector3f accelBias;
Vector3f wind; Vector3f wind;
Vector3f magNED; Vector3f magNED;
Vector3f magXYZ; Vector3f magXYZ;
getAccelBias(_core,accelBias); getAccelBias(accelBias);
getWind(_core,wind); getWind(wind);
getMagNED(_core,magNED); getMagNED(magNED);
getMagXYZ(_core,magXYZ); getMagXYZ(magXYZ);
const struct log_XKF2 pkt2{ const struct log_XKF2 pkt2{
LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
accBiasX : (int16_t)(100*accelBias.x), accBiasX : (int16_t)(100*accelBias.x),
accBiasY : (int16_t)(100*accelBias.y), accBiasY : (int16_t)(100*accelBias.y),
accBiasZ : (int16_t)(100*accelBias.z), accBiasZ : (int16_t)(100*accelBias.z),
@ -76,26 +77,22 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const
AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
} }
void NavEKF3::Log_Write_XKFS(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
{ {
// Write sensor selection EKF packet // Write sensor selection EKF packet
uint8_t magIndex = getActiveMag(_core);
uint8_t baroIndex = getActiveBaro(_core);
uint8_t GPSIndex = getActiveGPS(_core);
uint8_t airspeedIndex = getActiveAirspeed(_core);
const struct log_EKFS pkt { const struct log_EKFS pkt {
LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG), LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
mag_index : (uint8_t)(magIndex), mag_index : magSelectIndex,
baro_index : (uint8_t)(baroIndex), baro_index : selected_baro,
gps_index : (uint8_t)(GPSIndex), gps_index : selected_gps,
airspeed_index : (uint8_t)(airspeedIndex), airspeed_index : getActiveAirspeed()
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKF3(uint64_t time_us) const
{ {
// Write third EKF packet // Write third EKF packet
Vector3f velInnov; Vector3f velInnov;
@ -103,11 +100,11 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
Vector3f magInnov; Vector3f magInnov;
float tasInnov = 0; float tasInnov = 0;
float yawInnov = 0; float yawInnov = 0;
getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov); getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
const struct log_NKF3 pkt3{ const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
innovVN : (int16_t)(100*velInnov.x), innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y), innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z), innovVD : (int16_t)(100*velInnov.z),
@ -119,13 +116,13 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
innovMZ : (int16_t)(magInnov.z), innovMZ : (int16_t)(magInnov.z),
innovYaw : (int16_t)(100*degrees(yawInnov)), innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov), innovVT : (int16_t)(100*tasInnov),
rerr : coreRelativeErrors[_core], rerr : frontend->coreRelativeErrors[core_index],
errorScore : coreErrorScores[_core] errorScore : frontend->coreErrorScores[core_index]
}; };
AP::logger().WriteBlock(&pkt3, sizeof(pkt3)); AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
} }
void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
{ {
// Write fourth EKF packet // Write fourth EKF packet
float velVar = 0; float velVar = 0;
@ -133,90 +130,77 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const
float hgtVar = 0; float hgtVar = 0;
Vector3f magVar; Vector3f magVar;
float tasVar = 0; float tasVar = 0;
uint16_t _faultStatus=0;
Vector2f offset; Vector2f offset;
uint16_t faultStatus=0;
uint8_t timeoutStatus=0; uint8_t timeoutStatus=0;
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {}; nav_gps_status gpsStatus {};
getVariances(_core,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(_core,faultStatus); getFilterFaults(_faultStatus);
getFilterTimeouts(_core,timeoutStatus); getFilterTimeouts(timeoutStatus);
getFilterStatus(_core,solutionStatus); getFilterStatus(solutionStatus);
getFilterGpsStatus(_core,gpsStatus); getFilterGpsStatus(gpsStatus);
float tiltError; float tiltError;
getTiltError(_core,tiltError); getTiltError(tiltError);
uint8_t primaryIndex = getPrimaryCoreIndex();
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,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
sqrtvarV : (int16_t)(100*velVar), sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar), sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar), sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar), sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar), sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError, tiltErr : tiltError,
offsetNorth : (int8_t)(offset.x), offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y), offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus), faults : _faultStatus,
timeouts : (uint8_t)(timeoutStatus), timeouts : timeoutStatus,
solution : (uint32_t)(solutionStatus.value), solution : solutionStatus.value,
gps : (uint16_t)(gpsStatus.value), gps : gpsStatus.value,
primary : (int8_t)primaryIndex primary : frontend->getPrimaryCoreIndex()
}; };
AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
} }
void NavEKF3::Log_Write_XKF5(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
{ {
if (_core != primary) { if (core_index != frontend->primary) {
// log only primary instance for now // log only primary instance for now
return; return;
} }
// Write fifth EKF packet
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
float HAGL=0; // height above ground level
float rngInnov=0; // range finder innovations
float range=0; // measured range
float gndOffsetErr=0; // filter ground offset state error
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
getFlowDebug(_core, normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
getOutputTrackingError(_core, predictorErrors);
const struct log_NKF5 pkt5{ const struct log_NKF5 pkt5{
LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
normInnov : (uint8_t)(MIN(100*normInnov,255)), normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)), // normalised innovation variance ratio for optical flow observations fused by the main nav filter
FIX : (int16_t)(1000*flowInnovX), FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*flowInnovY), FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
AFI : (int16_t)(1000*auxFlowInnov), AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)), // optical flow LOS rate innovation from terrain offset estimator
HAGL : (int16_t)(100*HAGL), HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level
offset : (int16_t)(100*gndOffset), offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*rngInnov), RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*range), meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*gndOffsetErr), errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
angErr : (float)predictorErrors.x, angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)predictorErrors.y, velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)predictorErrors.z posErr : (float)outputTrackError.z // output predictor position tracking error
}; };
AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
} }
void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_Quaternion(uint64_t time_us) const
{ {
// log quaternion // log quaternion
Quaternion quat; Quaternion quat;
getQuaternion(_core, quat); getQuaternion( quat);
const struct log_Quaternion pktq1{ const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG), LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
q1 : quat.q1, q1 : quat.q1,
q2 : quat.q2, q2 : quat.q2,
q3 : quat.q3, q3 : quat.q3,
@ -225,64 +209,68 @@ void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const
AP::logger().WriteBlock(&pktq1, sizeof(pktq1)); AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
} }
void NavEKF3::Log_Write_Beacon(uint8_t _core, uint64_t time_us) const // logs beacon information, one beacon per call
void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
{ {
if (_core != primary) { if (core_index != frontend->primary) {
// log only primary instance for now // log only primary instance for now
return; return;
} }
// write range beacon fusion debug packet if the range value is non-zero if (!statesInitialised || N_beacons == 0) {
uint8_t ID; return;
float rng;
float innovVar;
float innov;
float testRatio;
Vector3f beaconPosNED;
float bcnPosOffsetHigh;
float bcnPosOffsetLow;
Vector3f posNED;
if (getRangeBeaconDebug(_core, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) {
if (rng > 0.0f) {
const struct log_RngBcnDebug pkt10{
LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG),
time_us : time_us,
core : DAL_CORE(_core),
ID : (uint8_t)ID,
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)),
testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*beaconPosNED.x),
beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z),
offsetHigh : (int16_t)(100*bcnPosOffsetHigh),
offsetLow : (int16_t)(100*bcnPosOffsetLow),
posN : (int16_t)(100*posNED.x),
posE : (int16_t)(100*posNED.y),
posD : (int16_t)(100*posNED.z)
};
AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
}
} }
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
if (rngBcnFuseDataReportIndex >= N_beacons) {
rngBcnFuseDataReportIndex = 0;
}
const rngBcnFusionReport_t &report = rngBcnFusionReport[rngBcnFuseDataReportIndex];
// write range beacon fusion debug packet if the range value is non-zero
if (report.rng <= 0.0f) {
rngBcnFuseDataReportIndex++;
return;
}
const struct log_RngBcnDebug pkt10{
LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG),
time_us : time_us,
core : DAL_CORE(core_index),
ID : rngBcnFuseDataReportIndex,
rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)),
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z),
offsetHigh : (int16_t)(100*bcnPosDownOffsetMax),
offsetLow : (int16_t)(100*bcnPosDownOffsetMin),
posN : (int16_t)(100*receiverPos.x),
posE : (int16_t)(100*receiverPos.y),
posD : (int16_t)(100*receiverPos.z)
};
AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
rngBcnFuseDataReportIndex++;
} }
void NavEKF3::Log_Write_BodyOdom(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
{ {
if (_core != primary) { if (core_index != frontend->primary) {
// log only primary instance for now // log only primary instance for now
return; return;
} }
Vector3f velBodyInnov,velBodyInnovVar; Vector3f velBodyInnov,velBodyInnovVar;
static uint32_t lastUpdateTime_ms = 0; static uint32_t lastUpdateTime_ms = 0;
uint32_t updateTime_ms = getBodyFrameOdomDebug(_core, velBodyInnov, velBodyInnovVar); uint32_t updateTime_ms = getBodyFrameOdomDebug( velBodyInnov, velBodyInnovVar);
if (updateTime_ms > lastUpdateTime_ms) { if (updateTime_ms > lastUpdateTime_ms) {
const struct log_ekfBodyOdomDebug pkt11{ const struct log_ekfBodyOdomDebug pkt11{
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG), LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
velInnovX : velBodyInnov.x, velInnovX : velBodyInnov.x,
velInnovY : velBodyInnov.y, velInnovY : velBodyInnov.y,
velInnovZ : velBodyInnov.z, velInnovZ : velBodyInnov.z,
@ -295,9 +283,9 @@ void NavEKF3::Log_Write_BodyOdom(uint8_t _core, uint64_t time_us) const
} }
} }
void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const
{ {
if (_core != primary) { if (core_index != frontend->primary) {
// log only primary instance for now // log only primary instance for now
return; return;
} }
@ -305,42 +293,40 @@ void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const
static uint32_t lastEkfStateVarLogTime_ms = 0; static uint32_t lastEkfStateVarLogTime_ms = 0;
if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) { if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) {
lastEkfStateVarLogTime_ms = AP::dal().millis(); lastEkfStateVarLogTime_ms = AP::dal().millis();
float stateVar[24];
getStateVariances(_core, stateVar);
const struct log_ekfStateVar pktv1{ const struct log_ekfStateVar pktv1{
LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG), LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
v00 : stateVar[0], v00 : P[0][0],
v01 : stateVar[1], v01 : P[1][1],
v02 : stateVar[2], v02 : P[2][2],
v03 : stateVar[3], v03 : P[3][3],
v04 : stateVar[4], v04 : P[4][4],
v05 : stateVar[5], v05 : P[5][5],
v06 : stateVar[6], v06 : P[6][6],
v07 : stateVar[7], v07 : P[7][7],
v08 : stateVar[8], v08 : P[8][8],
v09 : stateVar[9], v09 : P[9][9],
v10 : stateVar[10], v10 : P[10][10],
v11 : stateVar[11] v11 : P[11][11]
}; };
AP::logger().WriteBlock(&pktv1, sizeof(pktv1)); AP::logger().WriteBlock(&pktv1, sizeof(pktv1));
const struct log_ekfStateVar pktv2{ const struct log_ekfStateVar pktv2{
LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG), LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core), core : DAL_CORE(core_index),
v00 : stateVar[12], v00 : P[12][12],
v01 : stateVar[13], v01 : P[13][13],
v02 : stateVar[14], v02 : P[14][14],
v03 : stateVar[15], v03 : P[15][15],
v04 : stateVar[16], v04 : P[16][16],
v05 : stateVar[17], v05 : P[17][17],
v06 : stateVar[18], v06 : P[18][18],
v07 : stateVar[19], v07 : P[19][19],
v08 : stateVar[20], v08 : P[20][20],
v09 : stateVar[21], v09 : P[21][21],
v10 : stateVar[22], v10 : P[22][22],
v11 : stateVar[23] v11 : P[23][23]
}; };
AP::logger().WriteBlock(&pktv2, sizeof(pktv2)); AP::logger().WriteBlock(&pktv2, sizeof(pktv2));
} }
@ -360,35 +346,40 @@ void NavEKF3::Log_Write()
uint64_t time_us = AP::dal().micros64(); uint64_t time_us = AP::dal().micros64();
// note that several of these functions exit-early if they're not
// attempting to log the primary core.
for (uint8_t i=0; i<activeCores(); i++) { for (uint8_t i=0; i<activeCores(); i++) {
Log_Write_XKF1(i, time_us); core[i].Log_Write(time_us);
Log_Write_XKF2(i, time_us);
Log_Write_XKF3(i, time_us);
Log_Write_XKF4(i, time_us);
Log_Write_XKF5(i, time_us);
Log_Write_XKFS(i, time_us);
Log_Write_Quaternion(i, time_us);
Log_Write_GSF(i, time_us);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(i, time_us);
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(i, time_us);
// log state variances every 0.49s
Log_Write_State_Variances(i, time_us);
Log_Write_Timing(i, time_us);
} }
AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3); AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3);
} }
void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write(uint64_t time_us)
{
// note that several of these functions exit-early if they're not
// attempting to log the primary core.
Log_Write_XKF1(time_us);
Log_Write_XKF2(time_us);
Log_Write_XKF3(time_us);
Log_Write_XKF4(time_us);
Log_Write_XKF5(time_us);
Log_Write_XKFS(time_us);
Log_Write_Quaternion(time_us);
Log_Write_GSF(time_us);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(time_us);
// log state variances every 0.49s
Log_Write_State_Variances(time_us);
Log_Write_Timing(time_us);
}
void NavEKF3_core::Log_Write_Timing(uint64_t time_us)
{ {
// log EKF timing statistics every 5s // log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0; static uint32_t lastTimingLogTime_ms = 0;
@ -397,13 +388,10 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
} }
lastTimingLogTime_ms = AP::dal().millis(); lastTimingLogTime_ms = AP::dal().millis();
struct ekf_timing timing;
getTimingStatistics(_core, timing);
const struct log_XKT xkt{ const struct log_XKT xkt{
LOG_PACKET_HEADER_INIT(LOG_XKT_MSG), LOG_PACKET_HEADER_INIT(LOG_XKT_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : core_index,
timing_count : timing.count, timing_count : timing.count,
dtIMUavg_min : timing.dtIMUavg_min, dtIMUavg_min : timing.dtIMUavg_min,
dtIMUavg_max : timing.dtIMUavg_max, dtIMUavg_max : timing.dtIMUavg_max,
@ -414,11 +402,17 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
delVelDT_min : timing.delVelDT_min, delVelDT_min : timing.delVelDT_min,
delVelDT_max : timing.delVelDT_max, delVelDT_max : timing.delVelDT_max,
}; };
memset(&timing, 0, sizeof(timing));
AP::logger().WriteBlock(&xkt, sizeof(xkt)); AP::logger().WriteBlock(&xkt, sizeof(xkt));
} }
void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const void NavEKF3_core::Log_Write_GSF(uint64_t time_us)
{ {
if (yawEstimator == nullptr) {
return;
}
float yaw_composite; float yaw_composite;
float yaw_composite_variance; float yaw_composite_variance;
float yaw[N_MODELS_EKFGSF]; float yaw[N_MODELS_EKFGSF];
@ -426,7 +420,9 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
float ive[N_MODELS_EKFGSF]; float ive[N_MODELS_EKFGSF];
float wgt[N_MODELS_EKFGSF]; float wgt[N_MODELS_EKFGSF];
if (getDataEKFGSF(_core, yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) { if (!yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) {
return;
}
// @LoggerMessage: XKY0 // @LoggerMessage: XKY0
// @Description: EKF3 Yaw Estimator States // @Description: EKF3 Yaw Estimator States
@ -497,5 +493,4 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
ive[2], ive[2],
ive[3], ive[3],
ive[4]); ive[4]);
}
} }

7
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1175,13 +1175,6 @@ void NavEKF3_core::updateTimingStatistics(void)
timing.count++; timing.count++;
} }
// get timing statistics structure
void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing)
{
_timing = timing;
memset(&timing, 0, sizeof(timing));
}
/* /*
update estimates of inactive bias states. This keeps inactive IMUs update estimates of inactive bias states. This keeps inactive IMUs
as hot-spares so we can switch to them without causing a jump in the as hot-spares so we can switch to them without causing a jump in the

81
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -59,20 +59,6 @@ float NavEKF3_core::errorScore() const
return score; return score;
} }
// return data for debugging optical flow fusion
void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
{
varFlow = MAX(flowTestRatio[0],flowTestRatio[1]);
gndOffset = terrainState;
flowInnovX = innovOptFlow[0];
flowInnovY = innovOptFlow[1];
auxInnov = norm(auxFlowObsInnov.x,auxFlowObsInnov.y);
HAGL = terrainState - stateStruct.position.z;
rngInnov = innovRng;
range = rangeDataDelayed.rng;
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
}
// return data for debugging body frame odometry fusion // return data for debugging body frame odometry fusion
uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar) uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
{ {
@ -85,34 +71,6 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn
return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms); return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
} }
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED)
{
// if the states have not been initialised or we have not received any beacon updates then return zeros
if (!statesInitialised || N_beacons == 0) {
return false;
}
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
if (rngBcnFuseDataReportIndex >= N_beacons) {
rngBcnFuseDataReportIndex = 0;
}
// Output the fusion status data for the specified beacon
ID = rngBcnFuseDataReportIndex; // beacon identifier
rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng; // measured range to beacon (m)
innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m)
innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2)
testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon receiver NED position (m)
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate (m)
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate (m)
posNED = receiverPos; // beacon system NED offset (m)
rngBcnFuseDataReportIndex++;
return true;
}
// provides the height limit to be observed by the control loops // provides the height limit to be observed by the control loops
// returns false if no height limiting is required // returns false if no height limiting is required
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
@ -450,23 +408,6 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
} }
// return the index for the active magnetometer // return the index for the active magnetometer
uint8_t NavEKF3_core::getActiveMag() const
{
return (uint8_t)magSelectIndex;
}
// return the index for the active barometer
uint8_t NavEKF3_core::getActiveBaro() const
{
return (uint8_t)selected_baro;
}
// return the index for the active GPS
uint8_t NavEKF3_core::getActiveGPS() const
{
return (uint8_t)selected_gps;
}
// return the index for the active airspeed // return the index for the active airspeed
uint8_t NavEKF3_core::getActiveAirspeed() const uint8_t NavEKF3_core::getActiveAirspeed() const
{ {
@ -505,14 +446,6 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
offset = posResetNE; offset = posResetNE;
} }
// return the diagonals from the covariance matrix
void NavEKF3_core::getStateVariances(float stateVar[24])
{
for (uint8_t i=0; i<24; i++) {
stateVar[i] = P[i][i];
}
}
// get a particular source's velocity innovations // get a particular source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments // 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 bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
@ -693,17 +626,3 @@ uint8_t NavEKF3_core::getFramesSincePredict(void) const
{ {
return framesSincePredict; return framesSincePredict;
} }
// publish output observer angular, velocity and position tracking error
void NavEKF3_core::getOutputTrackingError(Vector3f &error) const
{
error = outputTrackError;
}
bool NavEKF3_core::getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
if (yawEstimator != nullptr) {
return yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight);
}
return false;
}

56
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -163,9 +163,6 @@ public:
void getMagXYZ(Vector3f &magXYZ) const; void getMagXYZ(Vector3f &magXYZ) const;
// return the index for the active sensors // return the index for the active sensors
uint8_t getActiveMag() const;
uint8_t getActiveBaro() const;
uint8_t getActiveGPS() const;
uint8_t getActiveAirspeed() const; uint8_t getActiveAirspeed() const;
// Return estimated magnetometer offsets // Return estimated magnetometer offsets
@ -208,9 +205,6 @@ public:
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// return the diagonals from the covariance matrix
void getStateVariances(float stateVar[24]);
// get a particular source's velocity innovations // get a particular source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments // 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; bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
@ -228,9 +222,6 @@ public:
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// return data for debugging optical flow fusion
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
/* /*
* Write body frame linear and angular displacement measurements from a visual odometry sensor * Write body frame linear and angular displacement measurements from a visual odometry sensor
* *
@ -265,23 +256,6 @@ public:
*/ */
uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar); uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar);
/*
Returns the following data for debugging range beacon fusion
ID : beacon identifier
rng : measured range to beacon (m)
innov : range innovation (m)
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
offsetHigh : high hypothesis for range beacons system vertical offset (m)
offsetLow : low hypothesis for range beacons system vertical offset (m)
posNED : North,East,Down position estimate of receiver from 3-state filter
returns true if data could be found, false if it could not
*/
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED);
/* /*
* Writes the measurement from a yaw angle sensor * Writes the measurement from a yaw angle sensor
* *
@ -406,16 +380,10 @@ public:
// this is used by other instances to level load // this is used by other instances to level load
uint8_t getFramesSincePredict(void) const; uint8_t getFramesSincePredict(void) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(Vector3f &error) const;
// get the IMU index. For now we return the gyro index, as that is most // get the IMU index. For now we return the gyro index, as that is most
// critical for use by other subsystems. // critical for use by other subsystems.
uint8_t getIMUIndex(void) const { return gyro_index_active; } uint8_t getIMUIndex(void) const { return gyro_index_active; }
// get timing statistics structure
void getTimingStatistics(struct ekf_timing &timing);
// values for EK3_MAG_CAL // values for EK3_MAG_CAL
enum class MagCal { enum class MagCal {
WHEN_FLYING = 0, WHEN_FLYING = 0,
@ -429,10 +397,6 @@ public:
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
bool using_external_yaw(void) const; bool using_external_yaw(void) const;
// get solution data for the EKF-GSF emergency yaw estimator
// return false if data not available
bool getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed); void writeDefaultAirSpeed(float airspeed);
@ -449,7 +413,9 @@ public:
bool have_aligned_yaw(void) const { bool have_aligned_yaw(void) const {
return yawAlignComplete; return yawAlignComplete;
} }
void Log_Write(uint64_t time_us);
private: private:
EKFGSF_yaw *yawEstimator; EKFGSF_yaw *yawEstimator;
AP_DAL &dal; AP_DAL &dal;
@ -1291,7 +1257,7 @@ private:
// Range Beacon Fusion Debug Reporting // Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct { struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m) float rng; // measured range to beacon (m)
float innov; // range innovation (m) float innov; // range innovation (m)
float innovVar; // innovation variance (m^2) float innovVar; // innovation variance (m^2)
@ -1483,4 +1449,18 @@ private:
bool posxy_source_reset; // true when the horizontal position source has changed but the position has not yet been reset bool posxy_source_reset; // true when the horizontal position source has changed but the position has not yet been reset
AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change) AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change)
bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset
// logging functions shared by cores:
void Log_Write_XKF1(uint64_t time_us) const;
void Log_Write_XKF2(uint64_t time_us) const;
void Log_Write_XKF3(uint64_t time_us) const;
void Log_Write_XKF4(uint64_t time_us) const;
void Log_Write_XKF5(uint64_t time_us) const;
void Log_Write_XKFS(uint64_t time_us) const;
void Log_Write_Quaternion(uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us);
void Log_Write_BodyOdom(uint64_t time_us);
void Log_Write_State_Variances(uint64_t time_us) const;
void Log_Write_Timing(uint64_t time_us);
void Log_Write_GSF(uint64_t time_us);
}; };