Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: factor out and logging functions, reuse for 2nd core

Browse Source
This commit is contained in:
Peter Barker 2019-07-03 10:31:19 +10:00 committed by Andrew Tridgell
parent b6efd0bea1
commit 6a0a466f42
2 changed files with 98 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -29,6 +29,7 @@
#include <AP_Airspeed/AP_Airspeed.h> #include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Compass/AP_Compass.h> #include <AP_Compass/AP_Compass.h>
#include <AP_RangeFinder/AP_RangeFinder.h> #include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Logger/LogStructure.h>
class NavEKF2_core; class NavEKF2_core;
class AP_AHRS; class AP_AHRS;
@ -499,4 +500,13 @@ private:
// new_primary - index of the ekf instance that we are about to switch to as the primary // new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary // old_primary - index of the ekf instance that we are currently using as the primary
void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary); void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary);
// logging functions shared by cores:
void Log_Write_EKF1(uint8_t core, LogMessages msg_id, uint64_t time_us) const;
void Log_Write_NKF2(uint8_t core, LogMessages msg_id, uint64_t time_us) const;
void Log_Write_NKF3(uint8_t core, LogMessages msg_id, uint64_t time_us) const;
void Log_Write_NKF4(uint8_t core, LogMessages msg_id, uint64_t time_us) const;
void Log_Write_NKF5(uint64_t time_us) const;
void Log_Write_Quaternion(uint8_t core, LogMessages msg_id, uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us) const;
}; };

254
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -3,16 +3,8 @@
#include <AP_HAL/HAL.h> #include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
void NavEKF2::Log_Write() void NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{ {
// only log if enabled
if (activeCores() <= 0) {
return;
}
AP_Logger &logger = AP::logger();
uint64_t time_us = AP_HAL::micros64();
// Write first EKF packet // Write first EKF packet
Vector3f euler; Vector3f euler;
Vector2f posNE; Vector2f posNE;
@ -21,17 +13,17 @@ void NavEKF2::Log_Write()
Vector3f gyroBias; Vector3f gyroBias;
float posDownDeriv; float posDownDeriv;
Location originLLH; Location originLLH;
getEulerAngles(0,euler); getEulerAngles(_core,euler);
getVelNED(0,velNED); getVelNED(_core,velNED);
getPosNE(0,posNE); getPosNE(_core,posNE);
getPosD(0,posD); getPosD(_core,posD);
getGyroBias(0,gyroBias); getGyroBias(_core,gyroBias);
posDownDeriv = getPosDownDerivative(0); posDownDeriv = getPosDownDerivative(_core);
if (!getOriginLLH(0,originLLH)) { if (!getOriginLLH(_core,originLLH)) {
originLLH.alt = 0; originLLH.alt = 0;
} }
struct log_EKF1 pkt = { const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG), LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us, time_us : time_us,
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)
@ -48,22 +40,25 @@ void NavEKF2::Log_Write()
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
}; };
logger.WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void NavEKF2::Log_Write_NKF2(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write second EKF packet // Write second EKF packet
float azbias = 0; float azbias = 0;
Vector3f wind; Vector3f wind;
Vector3f magNED; Vector3f magNED;
Vector3f magXYZ; Vector3f magXYZ;
Vector3f gyroScaleFactor; Vector3f gyroScaleFactor;
uint8_t magIndex = getActiveMag(0); uint8_t magIndex = getActiveMag(_core);
getAccelZBias(0,azbias); getAccelZBias(_core,azbias);
getWind(0,wind); getWind(_core,wind);
getMagNED(0,magNED); getMagNED(_core,magNED);
getMagXYZ(0,magXYZ); getMagXYZ(_core,magXYZ);
getGyroScaleErrorPercentage(0,gyroScaleFactor); getGyroScaleErrorPercentage(_core,gyroScaleFactor);
struct log_NKF2 pkt2 = { const struct log_NKF2 pkt2{
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG), LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us, time_us : time_us,
AZbias : (int8_t)(100*azbias), AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x), scaleX : (int16_t)(100*gyroScaleFactor.x),
@ -79,17 +74,20 @@ void NavEKF2::Log_Write()
magZ : (int16_t)(magXYZ.z), magZ : (int16_t)(magXYZ.z),
index : (uint8_t)(magIndex) index : (uint8_t)(magIndex)
}; };
logger.WriteBlock(&pkt2, sizeof(pkt2)); AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}
void NavEKF2::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write third EKF packet // Write third EKF packet
Vector3f velInnov; Vector3f velInnov;
Vector3f posInnov; Vector3f posInnov;
Vector3f magInnov; Vector3f magInnov;
float tasInnov = 0; float tasInnov = 0;
float yawInnov = 0; float yawInnov = 0;
getInnovations(0,velInnov, posInnov, magInnov, tasInnov, yawInnov); getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov);
struct log_NKF3 pkt3 = { const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG), LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us, time_us : time_us,
innovVN : (int16_t)(100*velInnov.x), innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y), innovVE : (int16_t)(100*velInnov.y),
@ -103,8 +101,11 @@ void NavEKF2::Log_Write()
innovYaw : (int16_t)(100*degrees(yawInnov)), innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov) innovVT : (int16_t)(100*tasInnov)
}; };
logger.WriteBlock(&pkt3, sizeof(pkt3)); AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
}
void NavEKF2::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write fourth EKF packet // Write fourth EKF packet
float velVar = 0; float velVar = 0;
float posVar = 0; float posVar = 0;
@ -116,17 +117,17 @@ void NavEKF2::Log_Write()
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(0,velVar, posVar, hgtVar, magVar, tasVar, offset); getVariances(_core,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(0,faultStatus); getFilterFaults(_core,faultStatus);
getFilterTimeouts(0,timeoutStatus); getFilterTimeouts(_core,timeoutStatus);
getFilterStatus(0,solutionStatus); getFilterStatus(_core,solutionStatus);
getFilterGpsStatus(0,gpsStatus); getFilterGpsStatus(_core,gpsStatus);
float tiltError; float tiltError;
getTiltError(0,tiltError); getTiltError(_core,tiltError);
int8_t primaryIndex = getPrimaryCoreIndex(); int8_t primaryIndex = getPrimaryCoreIndex();
struct log_NKF4 pkt4 = { const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG), LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us, time_us : time_us,
sqrtvarV : (int16_t)(100*velVar), sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar), sqrtvarP : (int16_t)(100*posVar),
@ -142,8 +143,11 @@ void NavEKF2::Log_Write()
gps : (uint16_t)(gpsStatus.value), gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex primary : (int8_t)primaryIndex
}; };
logger.WriteBlock(&pkt4, sizeof(pkt4)); AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
}
void NavEKF2::Log_Write_NKF5(uint64_t time_us) const
{
// Write fifth EKF packet - take data from the primary instance // Write fifth EKF packet - take data from the primary instance
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter 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 gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
@ -156,7 +160,7 @@ void NavEKF2::Log_Write()
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
getOutputTrackingError(-1,predictorErrors); getOutputTrackingError(-1,predictorErrors);
struct log_NKF5 pkt5 = { const struct log_NKF5 pkt5{
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : time_us, time_us : time_us,
normInnov : (uint8_t)(MIN(100*normInnov,255)), normInnov : (uint8_t)(MIN(100*normInnov,255)),
@ -172,138 +176,27 @@ void NavEKF2::Log_Write()
velErr : (float)predictorErrors.y, velErr : (float)predictorErrors.y,
posErr : (float)predictorErrors.z posErr : (float)predictorErrors.z
}; };
logger.WriteBlock(&pkt5, sizeof(pkt5)); AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
}
void NavEKF2::Log_Write_Quaternion(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// log quaternion // log quaternion
Quaternion quat; Quaternion quat;
getQuaternion(0, quat); getQuaternion(_core, quat);
struct log_Quaternion pktq1 = { const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(LOG_NKQ1_MSG), LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us, time_us : time_us,
q1 : quat.q1, q1 : quat.q1,
q2 : quat.q2, q2 : quat.q2,
q3 : quat.q3, q3 : quat.q3,
q4 : quat.q4 q4 : quat.q4
}; };
logger.WriteBlock(&pktq1, sizeof(pktq1)); AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
}
// log innovations for the second IMU if enabled void NavEKF2::Log_Write_Beacon(uint64_t time_us) const
if (activeCores() >= 2) { {
// Write 6th EKF packet
getEulerAngles(1,euler);
getVelNED(1,velNED);
getPosNE(1,posNE);
getPosD(1,posD);
getGyroBias(1,gyroBias);
posDownDeriv = getPosDownDerivative(1);
if (!getOriginLLH(1,originLLH)) {
originLLH.alt = 0;
}
struct log_EKF1 pkt6 = {
LOG_PACKET_HEADER_INIT(LOG_NKF6_MSG),
time_us : time_us,
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)
velN : (float)(velNED.x), // velocity North (m/s)
velE : (float)(velNED.y), // velocity East (m/s)
velD : (float)(velNED.z), // velocity Down (m/s)
posD_dot : (float)(posDownDeriv), // first derivative of down position
posN : (float)(posNE.x), // metres North
posE : (float)(posNE.y), // metres East
posD : (float)(posD), // metres Down
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
logger.WriteBlock(&pkt6, sizeof(pkt6));
// Write 7th EKF packet
getAccelZBias(1,azbias);
getWind(1,wind);
getMagNED(1,magNED);
getMagXYZ(1,magXYZ);
getGyroScaleErrorPercentage(1,gyroScaleFactor);
magIndex = getActiveMag(1);
struct log_NKF2 pkt7 = {
LOG_PACKET_HEADER_INIT(LOG_NKF7_MSG),
time_us : time_us,
AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x),
scaleY : (int16_t)(100*gyroScaleFactor.y),
scaleZ : (int16_t)(100*gyroScaleFactor.z),
windN : (int16_t)(100*wind.x),
windE : (int16_t)(100*wind.y),
magN : (int16_t)(magNED.x),
magE : (int16_t)(magNED.y),
magD : (int16_t)(magNED.z),
magX : (int16_t)(magXYZ.x),
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z),
index : (uint8_t)(magIndex)
};
logger.WriteBlock(&pkt7, sizeof(pkt7));
// Write 8th EKF packet
getInnovations(1,velInnov, posInnov, magInnov, tasInnov, yawInnov);
struct log_NKF3 pkt8 = {
LOG_PACKET_HEADER_INIT(LOG_NKF8_MSG),
time_us : time_us,
innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z),
innovPN : (int16_t)(100*posInnov.x),
innovPE : (int16_t)(100*posInnov.y),
innovPD : (int16_t)(100*posInnov.z),
innovMX : (int16_t)(magInnov.x),
innovMY : (int16_t)(magInnov.y),
innovMZ : (int16_t)(magInnov.z),
innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov)
};
logger.WriteBlock(&pkt8, sizeof(pkt8));
// Write 9th EKF packet
getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset);
tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
getFilterFaults(1,faultStatus);
getFilterTimeouts(1,timeoutStatus);
getFilterStatus(1,solutionStatus);
getFilterGpsStatus(1,gpsStatus);
getTiltError(1,tiltError);
struct log_NKF4 pkt9 = {
LOG_PACKET_HEADER_INIT(LOG_NKF9_MSG),
time_us : time_us,
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,
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex
};
logger.WriteBlock(&pkt9, sizeof(pkt9));
getQuaternion(1, quat);
struct log_Quaternion pktq2 = {
LOG_PACKET_HEADER_INIT(LOG_NKQ2_MSG),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
logger.WriteBlock(&pktq2, sizeof(pktq2));
}
// write range beacon fusion debug packet if the range value is non-zero
if (AP::beacon() != nullptr) { if (AP::beacon() != nullptr) {
uint8_t ID; uint8_t ID;
float rng; float rng;
@ -332,10 +225,39 @@ void NavEKF2::Log_Write()
posE : 0, posE : 0,
posD : 0 posD : 0
}; };
logger.WriteBlock(&pkt10, sizeof(pkt10)); AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
} }
} }
} }
}
void NavEKF2::Log_Write()
{
// only log if enabled
if (activeCores() <= 0) {
return;
}
const uint64_t time_us = AP_HAL::micros64();
Log_Write_EKF1(0, LOG_NKF1_MSG, time_us);
Log_Write_NKF2(0, LOG_NKF2_MSG, time_us);
Log_Write_NKF3(0, LOG_NKF3_MSG, time_us);
Log_Write_NKF4(0, LOG_NKF4_MSG, time_us);
Log_Write_NKF5(time_us);
Log_Write_Quaternion(0, LOG_NKQ1_MSG, time_us);
// log innovations for the second IMU if enabled
if (activeCores() >= 2) {
Log_Write_EKF1(1, LOG_NKF6_MSG, time_us);
Log_Write_NKF2(1, LOG_NKF7_MSG, time_us);
Log_Write_NKF3(1, LOG_NKF8_MSG, time_us);
Log_Write_NKF4(1, LOG_NKF9_MSG, time_us);
Log_Write_Quaternion(1, LOG_NKQ2_MSG, time_us);
}
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(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;
@ -344,7 +266,7 @@ void NavEKF2::Log_Write()
struct ekf_timing timing; struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) { for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing); getTimingStatistics(i, timing);
logger.Write_EKF_Timing(i==0?"NKT1":"NKT2", time_us, timing); AP::logger().Write_EKF_Timing(i==0?"NKT1":"NKT2", time_us, timing);
} }
} }
} }