mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
AP_NavEKF3: make logging a core concern
Also dissolve some methods only used for logging
This commit is contained in:
Peter Barker
parent
914629351f
commit
e1a033b296
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
// 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
|
||||
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()
|
||||
}
|
||||
}
|
||||
|
||||
// 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_
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
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)
|
||||
{
|
||||
|
||||
@ -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:
|
||||
// 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:
|
||||
*/
|
||||
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:
|
||||
// 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:
|
||||
// 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:
|
||||
// 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;
|
||||
};
|
||||
|
||||
@ -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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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;
|
||||
}
|
||||
|
||||
// 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 (!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
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
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()
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
core[i].Log_Write(time_us);
|
||||
}
|
||||
|
||||
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
|
||||
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();
|
||||
|
||||
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
|
||||
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
|
||||
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
|
||||
ive[2],
|
||||
ive[3],
|
||||
ive[4]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
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
|
||||
}
|
||||
|
||||
// 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
|
||||
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
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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:
|
||||
// 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:
|
||||
// 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:
|
||||
*/
|
||||
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:
|
||||
// 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:
|
||||
|
||||
// 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:
|
||||
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:
|
||||
|
||||
// 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:
|
||||
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);
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user