mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-08 17:03:57 -04:00
AP_NavEKF: move GSF logging from EKF files into GSF files
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
65b780fad5
commit
a3dcf5a6ff
48
libraries/AP_NavEKF/EKFGSF_Logging.cpp
Normal file
48
libraries/AP_NavEKF/EKFGSF_Logging.cpp
Normal 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));
|
||||
}
|
||||
@ -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]);
|
||||
|
||||
@ -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:
|
||||
// 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:
|
||||
// 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
Normal file
88
libraries/AP_NavEKF/LogStructure.h
Normal 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 },
|
||||
Loading…
Reference in New Issue
Block a user