zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: make logging a core concern 

Also dissolve some methods only used for logging
c415-sdk
Peter Barker 4 years ago
parent
914629351f
commit
e1a033b296
6 changed files with 184 additions and 448 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 100
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 51
      libraries/AP_NavEKF3/AP_NavEKF3.h
  3. 337
      libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
  4. 7
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
  5. 81
      libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
  6. 56
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

100
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Unescape View File

@ -1239,39 +1239,6 @@ void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const @@ -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
uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const
{
@ -1408,15 +1375,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI @@ -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
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 @@ -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
// 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
@ -1538,15 +1487,6 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt @@ -1538,15 +1487,6 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
}
// 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
*
@ -1603,19 +1543,6 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec @@ -1603,19 +1543,6 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec
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
void NavEKF3::convert_parameters()
{
@ -1717,18 +1644,6 @@ 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
// 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
@ -2073,21 +1988,6 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_ @@ -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.
void NavEKF3::writeDefaultAirSpeed(float airspeed)
{

51
libraries/AP_NavEKF3/AP_NavEKF3.h
Unescape View File

@ -141,9 +141,6 @@ public: @@ -141,9 +141,6 @@ public:
// return the sensor in use for the specified 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;
// Return estimated magnetometer offsets
@ -189,16 +186,10 @@ public: @@ -189,16 +186,10 @@ public:
// 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;
// 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
// 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;
// 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;
@ -251,27 +242,6 @@ public: @@ -251,27 +242,6 @@ public:
*/
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
*
@ -395,9 +365,6 @@ public: @@ -395,9 +365,6 @@ public:
// allow the enable flag to be set by Replay
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
innovations. This is called when the vehicle code is about to
@ -424,10 +391,6 @@ public: @@ -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.
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
void convert_parameters();
@ -611,20 +574,6 @@ private: @@ -611,20 +574,6 @@ private:
// checks for alignment
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
AP_NavEKF_Source sources;
};

337
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
Unescape View File

@ -1,11 +1,12 @@ @@ -1,11 +1,12 @@
#include "AP_NavEKF3.h"
#include "AP_NavEKF3_core.h"
#include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.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
Vector3f euler;
@ -15,19 +16,19 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const @@ -15,19 +16,19 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const
Vector3f gyroBias;
float posDownDeriv;
Location originLLH;
getEulerAngles(_core,euler);
getVelNED(_core,velNED);
getPosNE(_core,posNE);
getPosD(_core,posD);
getGyroBias(_core,gyroBias);
posDownDeriv = getPosDownDerivative(_core);
if (!getOriginLLH(_core,originLLH)) {
getEulerAngles(euler);
getVelNED(velNED);
getPosNE(posNE);
getPosD(posD);
getGyroBias(gyroBias);
posDownDeriv = getPosDownDerivative();
if (!getOriginLLH(originLLH)) {
originLLH.alt = 0;
}
const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG),
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)
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)
@ -46,21 +47,21 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const @@ -46,21 +47,21 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const
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
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
getAccelBias(_core,accelBias);
getWind(_core,wind);
getMagNED(_core,magNED);
getMagXYZ(_core,magXYZ);
getAccelBias(accelBias);
getWind(wind);
getMagNED(magNED);
getMagXYZ(magXYZ);
const struct log_XKF2 pkt2{
LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
time_us : time_us,
core : DAL_CORE(_core),
core : DAL_CORE(core_index),
accBiasX : (int16_t)(100*accelBias.x),
accBiasY : (int16_t)(100*accelBias.y),
accBiasZ : (int16_t)(100*accelBias.z),
@ -76,26 +77,22 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const @@ -76,26 +77,22 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const
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
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 {
LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG),
time_us : time_us,
core : DAL_CORE(_core),
mag_index : (uint8_t)(magIndex),
baro_index : (uint8_t)(baroIndex),
gps_index : (uint8_t)(GPSIndex),
airspeed_index : (uint8_t)(airspeedIndex),
core : DAL_CORE(core_index),
mag_index : magSelectIndex,
baro_index : selected_baro,
gps_index : selected_gps,
airspeed_index : getActiveAirspeed()
};
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
Vector3f velInnov;
@ -103,11 +100,11 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const @@ -103,11 +100,11 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
Vector3f magInnov;
float tasInnov = 0;
float yawInnov = 0;
getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov);
getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG),
time_us : time_us,
core : DAL_CORE(_core),
core : DAL_CORE(core_index),
innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z),
@ -119,13 +116,13 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const @@ -119,13 +116,13 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
innovMZ : (int16_t)(magInnov.z),
innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov),
rerr : coreRelativeErrors[_core],
errorScore : coreErrorScores[_core]
rerr : frontend->coreRelativeErrors[core_index],
errorScore : frontend->coreErrorScores[core_index]
};
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
float velVar = 0;
@ -133,90 +130,77 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const @@ -133,90 +130,77 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const
float hgtVar = 0;
Vector3f magVar;
float tasVar = 0;
uint16_t _faultStatus=0;
Vector2f offset;
uint16_t faultStatus=0;
uint8_t timeoutStatus=0;
nav_filter_status solutionStatus {};
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);
getFilterFaults(_core,faultStatus);
getFilterTimeouts(_core,timeoutStatus);
getFilterStatus(_core,solutionStatus);
getFilterGpsStatus(_core,gpsStatus);
getFilterFaults(_faultStatus);
getFilterTimeouts(timeoutStatus);
getFilterStatus(solutionStatus);
getFilterGpsStatus(gpsStatus);
float tiltError;
getTiltError(_core,tiltError);
uint8_t primaryIndex = getPrimaryCoreIndex();
getTiltError(tiltError);
const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG),
time_us : time_us,
core : DAL_CORE(_core),
core : DAL_CORE(core_index),
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError,
tiltErr : tiltError,
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus),
solution : (uint32_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex
faults : _faultStatus,
timeouts : timeoutStatus,
solution : solutionStatus.value,
gps : gpsStatus.value,
primary : frontend->getPrimaryCoreIndex()
};
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
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{
LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
time_us : time_us,
core : DAL_CORE(_core),
normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY),
AFI : (int16_t)(1000*auxFlowInnov),
HAGL : (int16_t)(100*HAGL),
offset : (int16_t)(100*gndOffset),
RI : (int16_t)(100*rngInnov),
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr),
angErr : (float)predictorErrors.x,
velErr : (float)predictorErrors.y,
posErr : (float)predictorErrors.z
core : DAL_CORE(core_index),
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*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)), // optical flow LOS rate innovation from terrain offset estimator
HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level
offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)outputTrackError.z // output predictor position tracking error
};
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
Quaternion quat;
getQuaternion(_core, quat);
getQuaternion( quat);
const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG),
time_us : time_us,
core : DAL_CORE(_core),
core : DAL_CORE(core_index),
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
@ -225,64 +209,68 @@ void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const @@ -225,64 +209,68 @@ void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const
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
return;
}
if (!statesInitialised || N_beacons == 0) {
return;
}
// 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
uint8_t ID;
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));
}
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
return;
}
Vector3f velBodyInnov,velBodyInnovVar;
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) {
const struct log_ekfBodyOdomDebug pkt11{
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us,
core : DAL_CORE(_core),
core : DAL_CORE(core_index),
velInnovX : velBodyInnov.x,
velInnovY : velBodyInnov.y,
velInnovZ : velBodyInnov.z,
@ -295,9 +283,9 @@ void NavEKF3::Log_Write_BodyOdom(uint8_t _core, uint64_t time_us) const @@ -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
return;
}
@ -305,42 +293,40 @@ void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const @@ -305,42 +293,40 @@ void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const
static uint32_t lastEkfStateVarLogTime_ms = 0;
if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) {
lastEkfStateVarLogTime_ms = AP::dal().millis();
float stateVar[24];
getStateVariances(_core, stateVar);
const struct log_ekfStateVar pktv1{
LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
time_us : time_us,
core : DAL_CORE(_core),
v00 : stateVar[0],
v01 : stateVar[1],
v02 : stateVar[2],
v03 : stateVar[3],
v04 : stateVar[4],
v05 : stateVar[5],
v06 : stateVar[6],
v07 : stateVar[7],
v08 : stateVar[8],
v09 : stateVar[9],
v10 : stateVar[10],
v11 : stateVar[11]
core : DAL_CORE(core_index),
v00 : P[0][0],
v01 : P[1][1],
v02 : P[2][2],
v03 : P[3][3],
v04 : P[4][4],
v05 : P[5][5],
v06 : P[6][6],
v07 : P[7][7],
v08 : P[8][8],
v09 : P[9][9],
v10 : P[10][10],
v11 : P[11][11]
};
AP::logger().WriteBlock(&pktv1, sizeof(pktv1));
const struct log_ekfStateVar pktv2{
LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
time_us : time_us,
core : DAL_CORE(_core),
v00 : stateVar[12],
v01 : stateVar[13],
v02 : stateVar[14],
v03 : stateVar[15],
v04 : stateVar[16],
v05 : stateVar[17],
v06 : stateVar[18],
v07 : stateVar[19],
v08 : stateVar[20],
v09 : stateVar[21],
v10 : stateVar[22],
v11 : stateVar[23]
core : DAL_CORE(core_index),
v00 : P[12][12],
v01 : P[13][13],
v02 : P[14][14],
v03 : P[15][15],
v04 : P[16][16],
v05 : P[17][17],
v06 : P[18][18],
v07 : P[19][19],
v08 : P[20][20],
v09 : P[21][21],
v10 : P[22][22],
v11 : P[23][23]
};
AP::logger().WriteBlock(&pktv2, sizeof(pktv2));
}
@ -360,35 +346,40 @@ void NavEKF3::Log_Write() @@ -360,35 +346,40 @@ void NavEKF3::Log_Write()
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++) {
Log_Write_XKF1(i, 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);
core[i].Log_Write(time_us);
}
AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3);
}
Log_Write_XKFS(i, time_us);
Log_Write_Quaternion(i, time_us);
Log_Write_GSF(i, time_us);
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);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(i, time_us);
Log_Write_XKFS(time_us);
Log_Write_Quaternion(time_us);
Log_Write_GSF(time_us);
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(i, time_us);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);
// log state variances every 0.49s
Log_Write_State_Variances(i, time_us);
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(time_us);
Log_Write_Timing(i, time_us);
}
// log state variances every 0.49s
Log_Write_State_Variances(time_us);
AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3);
Log_Write_Timing(time_us);
}
void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
void NavEKF3_core::Log_Write_Timing(uint64_t time_us)
{
// log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
@ -397,13 +388,10 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const @@ -397,13 +388,10 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
}
lastTimingLogTime_ms = AP::dal().millis();
struct ekf_timing timing;
getTimingStatistics(_core, timing);
const struct log_XKT xkt{
LOG_PACKET_HEADER_INIT(LOG_XKT_MSG),
time_us : time_us,
core : _core,
core : core_index,
timing_count : timing.count,
dtIMUavg_min : timing.dtIMUavg_min,
dtIMUavg_max : timing.dtIMUavg_max,
@ -414,11 +402,17 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const @@ -414,11 +402,17 @@ void NavEKF3::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
delVelDT_min : timing.delVelDT_min,
delVelDT_max : timing.delVelDT_max,
};
memset(&timing, 0, sizeof(timing));
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_variance;
float yaw[N_MODELS_EKFGSF];
@ -426,7 +420,9 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const @@ -426,7 +420,9 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
float ive[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
// @Description: EKF3 Yaw Estimator States
@ -497,5 +493,4 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const @@ -497,5 +493,4 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
ive[2],
ive[3],
ive[4]);
}
}

7
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Unescape View File

@ -1175,13 +1175,6 @@ void NavEKF3_core::updateTimingStatistics(void) @@ -1175,13 +1175,6 @@ void NavEKF3_core::updateTimingStatistics(void)
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
as hot-spares so we can switch to them without causing a jump in the

81
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Unescape View File

@ -59,20 +59,6 @@ float NavEKF3_core::errorScore() const @@ -59,20 +59,6 @@ float NavEKF3_core::errorScore() const
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
uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
{
@ -85,34 +71,6 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn @@ -85,34 +71,6 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn
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
// 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
@ -450,23 +408,6 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const @@ -450,23 +408,6 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
}
// 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
uint8_t NavEKF3_core::getActiveAirspeed() const
{
@ -505,14 +446,6 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve @@ -505,14 +446,6 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
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
// 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
@ -693,17 +626,3 @@ uint8_t NavEKF3_core::getFramesSincePredict(void) const @@ -693,17 +626,3 @@ uint8_t NavEKF3_core::getFramesSincePredict(void) const
{
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
Unescape View File

@ -163,9 +163,6 @@ public: @@ -163,9 +163,6 @@ public:
void getMagXYZ(Vector3f &magXYZ) const;
// return the index for the active sensors
uint8_t getActiveMag() const;
uint8_t getActiveBaro() const;
uint8_t getActiveGPS() const;
uint8_t getActiveAirspeed() const;
// Return estimated magnetometer offsets
@ -208,9 +205,6 @@ public: @@ -208,9 +205,6 @@ public:
// 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;
// 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;
@ -228,9 +222,6 @@ public: @@ -228,9 +222,6 @@ public:
// 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);
// 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
*
@ -265,23 +256,6 @@ public: @@ -265,23 +256,6 @@ public:
*/
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
*
@ -406,16 +380,10 @@ public: @@ -406,16 +380,10 @@ public:
// this is used by other instances to level load
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
// critical for use by other subsystems.
uint8_t getIMUIndex(void) const { return gyro_index_active; }
// get timing statistics structure
void getTimingStatistics(struct ekf_timing &timing);
// values for EK3_MAG_CAL
enum class MagCal {
WHEN_FLYING = 0,
@ -429,10 +397,6 @@ public: @@ -429,10 +397,6 @@ public:
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check
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.
void writeDefaultAirSpeed(float airspeed);
@ -449,7 +413,9 @@ public: @@ -449,7 +413,9 @@ public:
bool have_aligned_yaw(void) const {
return yawAlignComplete;
}
void Log_Write(uint64_t time_us);
private:
EKFGSF_yaw *yawEstimator;
AP_DAL &dal;
@ -1291,7 +1257,7 @@ private: @@ -1291,7 +1257,7 @@ private:
// Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct {
struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m)
float innov; // range innovation (m)
float innovVar; // innovation variance (m^2)
@ -1483,4 +1449,18 @@ private: @@ -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
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
// 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);
};

Write Preview
Loading…
Cancel
Save