zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: move GSF logging from EKF files into GSF files

zr-v5.1
Peter Barker 4 years ago committed by Andrew Tridgell
parent
65b780fad5
commit
a3dcf5a6ff
4 changed files with 141 additions and 20 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. 48
      libraries/AP_NavEKF/EKFGSF_Logging.cpp
  2. 16
      libraries/AP_NavEKF/EKFGSF_yaw.cpp
  3. 9
      libraries/AP_NavEKF/EKFGSF_yaw.h
  4. 88
      libraries/AP_NavEKF/LogStructure.h

48
libraries/AP_NavEKF/EKFGSF_Logging.cpp
Unescape View File

@ -0,0 +1,48 @@ @@ -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
Unescape View File

@ -577,22 +577,6 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const @@ -577,22 +577,6 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
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)
{
float P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]);

9
libraries/AP_NavEKF/EKFGSF_yaw.h
Unescape View File

@ -5,6 +5,7 @@ @@ -5,6 +5,7 @@
#include <AP_NavEKF/AP_Nav_Common.h>
#include <AP_Math/AP_Math.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)
@ -30,10 +31,6 @@ public: @@ -30,10 +31,6 @@ public:
// set the gyro bias in rad/sec
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
// return false if yaw estimation is inactive
bool getYawData(float &yaw, float &yawVariance) const;
@ -42,6 +39,10 @@ public: @@ -42,6 +39,10 @@ public:
// return false if not available
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:
typedef float ftype;

88
libraries/AP_NavEKF/LogStructure.h
Unescape View File

@ -0,0 +1,88 @@ @@ -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 },
Write Preview
Loading…
Cancel
Save