Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: move GSF logging from EKF files into GSF files

Browse Source
This commit is contained in:
Peter Barker 2021-02-18 09:34:14 +11:00 committed by Andrew Tridgell
parent 65b780fad5
commit a3dcf5a6ff
4 changed files with 141 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_NavEKF/EKFGSF_Logging.cpp Normal file
View File

@ -0,0 +1,48 @@
#include "EKFGSF_yaw.h"
#include <AP_Logger/AP_Logger.h>
void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index)
{
if (!vel_fuse_running) {
return;
}
static_assert(N_MODELS_EKFGSF >= 5, "Logging will break on <5 EKFGSF models");
const struct log_KY0 ky0{
LOG_PACKET_HEADER_INIT(id0),
time_us : time_us,
core : core_index,
yaw_composite : GSF.yaw,
yaw_composite_variance : sqrtf(MAX(GSF.yaw_variance, 0.0f)),
yaw0 : EKF[0].X[2],
yaw1 : EKF[1].X[2],
yaw2 : EKF[2].X[2],
yaw3 : EKF[3].X[2],
yaw4 : EKF[4].X[2],
wgt0 : GSF.weights[0],
wgt1 : GSF.weights[1],
wgt2 : GSF.weights[2],
wgt3 : GSF.weights[3],
wgt4 : GSF.weights[4],
};
AP::logger().WriteBlock(&ky0, sizeof(ky0));
const struct log_KY1 ky1{
LOG_PACKET_HEADER_INIT(id1),
time_us : time_us,
core : core_index,
ivn0 : EKF[0].innov[0],
ivn1 : EKF[1].innov[0],
ivn2 : EKF[2].innov[0],
ivn3 : EKF[3].innov[0],
ivn4 : EKF[4].innov[0],
ive0 : EKF[0].innov[1],
ive1 : EKF[1].innov[1],
ive2 : EKF[2].innov[1],
ive3 : EKF[3].innov[1],
ive4 : EKF[4].innov[1],
};
AP::logger().WriteBlock(&ky1, sizeof(ky1));
}

16
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -577,22 +577,6 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
return normDist; return normDist;
} }
bool EKFGSF_yaw::getLogData(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 (vel_fuse_running) {
yaw_composite = GSF.yaw;
yaw_composite_variance = GSF.yaw_variance;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
yaw[mdl_idx] = EKF[mdl_idx].X[2];
innov_VN[mdl_idx] = EKF[mdl_idx].innov[0];
innov_VE[mdl_idx] = EKF[mdl_idx].innov[1];
weight[mdl_idx] = GSF.weights[mdl_idx];
}
return true;
}
return false;
}
void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx) void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
{ {
float P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]); float P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]);

9
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -5,6 +5,7 @@
#include <AP_NavEKF/AP_Nav_Common.h> #include <AP_NavEKF/AP_Nav_Common.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
#include <AP_Logger/LogStructure.h>
#define IMU_DT_MIN_SEC 0.001f // Minimum delta time between IMU samples (sec) #define IMU_DT_MIN_SEC 0.001f // Minimum delta time between IMU samples (sec)
@ -30,10 +31,6 @@ public:
// set the gyro bias in rad/sec // set the gyro bias in rad/sec
void setGyroBias(Vector3f &gyroBias); void setGyroBias(Vector3f &gyroBias);
// get solution data for logging
// return false if yaw estimation is inactive
bool getLogData(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;
// get yaw estimated and corresponding variance // get yaw estimated and corresponding variance
// return false if yaw estimation is inactive // return false if yaw estimation is inactive
bool getYawData(float &yaw, float &yawVariance) const; bool getYawData(float &yaw, float &yawVariance) const;
@ -42,6 +39,10 @@ public:
// return false if not available // return false if not available
bool getVelInnovLength(float &velInnovLength) const; bool getVelInnovLength(float &velInnovLength) const;
// log EKFGSF data on behalf of an EKF caller. id0 and id1 are the
// IDs of the messages to log, e.g. LOG_NKY0_MSG, LOG_NKY1_MSG
void Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index);
private: private:
typedef float ftype; typedef float ftype;

88
libraries/AP_NavEKF/LogStructure.h Normal file
View File

@ -0,0 +1,88 @@
#pragma once
#include <AP_Logger/LogStructure.h>
// @LoggerMessage: XKY0,NKY0
// @Description: EKF Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF core this data is for
// @Field: YC: GSF yaw estimate (rad)
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad)
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad)
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad)
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad)
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad)
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad)
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4
struct PACKED log_KY0 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
float yaw_composite;
float yaw_composite_variance;
float yaw0;
float yaw1;
float yaw2;
float yaw3;
float yaw4;
float wgt0;
float wgt1;
float wgt2;
float wgt3;
float wgt4;
};
// @LoggerMessage: XKY1,NKY1
// @Description: EKF Yaw Estimator Innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF core this data is for
// @Field: IVN0: North velocity innovation from individual EKF filter 0 (m/s)
// @Field: IVN1: North velocity innovation from individual EKF filter 1 (m/s)
// @Field: IVN2: North velocity innovation from individual EKF filter 2 (m/s)
// @Field: IVN3: North velocity innovation from individual EKF filter 3 (m/s)
// @Field: IVN4: North velocity innovation from individual EKF filter 4 (m/s)
// @Field: IVE0: East velocity innovation from individual EKF filter 0 (m/s)
// @Field: IVE1: East velocity innovation from individual EKF filter 1 (m/s)
// @Field: IVE2: East velocity innovation from individual EKF filter 2 (m/s)
// @Field: IVE3: East velocity innovation from individual EKF filter 3 (m/s)
// @Field: IVE4: East velocity innovation from individual EKF filter 4 (m/s)
struct PACKED log_KY1 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
float ivn0;
float ivn1;
float ivn2;
float ivn3;
float ivn4;
float ive0;
float ive1;
float ive2;
float ive3;
float ive4;
};
#define KY0_FMT "QBffffffffffff"
#define KY0_LABELS "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4"
#define KY0_UNITS "s#rrrrrrr-----"
#define KY0_MULTS "F-000000000000"
#define KY1_FMT "QBffffffffff"
#define KY1_LABELS "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4"
#define KY1_UNITS "s#nnnnnnnnnn"
#define KY1_MULTS "F-0000000000"
#define LOG_STRUCTURE_FROM_NAVEKF \
{ LOG_XKY0_MSG, sizeof(log_KY0), \
"XKY0", KY0_FMT, KY0_LABELS, KY0_UNITS, KY0_MULTS}, \
{ LOG_XKY1_MSG, sizeof(log_KY1), \
"XKY1", KY1_FMT, KY1_LABELS, KY1_UNITS, KY1_MULTS }, \
{ LOG_NKY0_MSG, sizeof(log_KY0), \
"NKY0", KY0_FMT, KY0_LABELS, KY0_UNITS, KY0_MULTS}, \
{ LOG_NKY1_MSG, sizeof(log_KY1), \
"NKY1", KY1_FMT, KY1_LABELS, KY1_UNITS, KY1_MULTS },