Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use core as instance number when logging EKF data #12997

Merged
merged 3 commits into from
Dec 17, 2019
Merged

Use core as instance number when logging EKF data #12997

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
df3d4ae
LogStructure: adjust for using instance numbers on NavEKF libraries
peterbarker Dec 6, 2019
dbb7159
AP_NavEKF2: use instance number when logging rather than multiple mes…
peterbarker Dec 6, 2019
1c5c0d9
AP_NavEKF3: use instance number when logging rather than multiple mes…
peterbarker Dec 6, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
97 changes: 25 additions & 72 deletions libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -433,6 +433,7 @@ struct PACKED log_POWR {
struct PACKED log_EKF1 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t roll;
int16_t pitch;
uint16_t yaw;
Expand All @@ -452,6 +453,7 @@ struct PACKED log_EKF1 {
struct PACKED log_EKF2 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int8_t Ratio;
int8_t AZ1bias;
int8_t AZ2bias;
Expand All @@ -468,6 +470,7 @@ struct PACKED log_EKF2 {
struct PACKED log_NKF2 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int8_t AZbias;
int16_t scaleX;
int16_t scaleY;
Expand All @@ -486,6 +489,7 @@ struct PACKED log_NKF2 {
struct PACKED log_NKF2a {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t accBiasX;
int16_t accBiasY;
int16_t accBiasZ;
Expand All @@ -503,6 +507,7 @@ struct PACKED log_NKF2a {
struct PACKED log_EKF3 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t innovVN;
int16_t innovVE;
int16_t innovVD;
Expand All @@ -518,6 +523,7 @@ struct PACKED log_EKF3 {
struct PACKED log_NKF3 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t innovVN;
int16_t innovVE;
int16_t innovVD;
Expand All @@ -534,6 +540,7 @@ struct PACKED log_NKF3 {
struct PACKED log_EKF4 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t sqrtvarV;
int16_t sqrtvarP;
int16_t sqrtvarH;
Expand All @@ -552,6 +559,7 @@ struct PACKED log_EKF4 {
struct PACKED log_NKF4 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
int16_t sqrtvarV;
int16_t sqrtvarP;
int16_t sqrtvarH;
Expand Down Expand Up @@ -601,6 +609,7 @@ struct PACKED log_NKF5 {
struct PACKED log_Quaternion {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
float q1;
float q2;
float q3;
Expand Down Expand Up @@ -1271,10 +1280,10 @@ struct PACKED log_Arm_Disarm {
#define PID_UNITS "s-------"
#define PID_MULTS "F-------"

#define QUAT_LABELS "TimeUS,Q1,Q2,Q3,Q4"
#define QUAT_FMT "Qffff"
#define QUAT_UNITS "s????"
#define QUAT_MULTS "F????"
#define QUAT_LABELS "TimeUS,C,Q1,Q2,Q3,Q4"
#define QUAT_FMT "QBffff"
#define QUAT_UNITS "s#????"
#define QUAT_MULTS "F-????"

#define CURR_LABELS "TimeUS,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res"
#define CURR_FMT "Qfffffcf"
Expand Down Expand Up @@ -1413,67 +1422,31 @@ struct PACKED log_Arm_Disarm {
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
{ LOG_NKF1_MSG, sizeof(log_EKF1), \
"NKF1","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnnnmmmkkkm", "FBBB0000000BBBB" }, \
"NKF1","QBccCfffffffccce","TimeUS,C,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "s#ddhnnnnmmmkkkm", "F-BBB0000000BBBB" }, \
{ LOG_NKF2_MSG, sizeof(log_NKF2), \
"NKF2","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s----nnGGGGGG-", "F----BBCCCCCC-" }, \
"NKF2","QBbccccchhhhhhB","TimeUS,C,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s#----nnGGGGGG-", "F-----BBCCCCCC-" }, \
{ LOG_NKF3_MSG, sizeof(log_NKF3), \
"NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGG??", "FBBBBBBCCCBB" }, \
"NKF3","QBcccccchhhcc","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "s#nnnmmmGGG??", "F-BBBBBBCCCBB" }, \
{ LOG_NKF4_MSG, sizeof(log_NKF4), \
"NKF4","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s------??-----", "F------??-----" }, \
"NKF4","QBcccccfbbHBHHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
{ LOG_NKF5_MSG, sizeof(log_NKF5), \
"NKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s----m???mrnm", "F----BBBBB000" }, \
{ LOG_NKF6_MSG, sizeof(log_EKF1), \
"NKF6","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_NKF7_MSG, sizeof(log_NKF2), \
"NKF7","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s----nnGGGGGG-", "F----BBCCCCCC-" }, \
{ LOG_NKF8_MSG, sizeof(log_NKF3), \
"NKF8","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_NKF9_MSG, sizeof(log_NKF4), \
"NKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \
"NKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s-m---mmmmmmmm", "F-B---BBBBBBBB" }, \
{ LOG_NKF11_MSG, sizeof(log_EKF1), \
"NK11","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_NKF12_MSG, sizeof(log_NKF2), \
"NK12","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s----nnGGGGGG-", "F----BBCCCCCC-" }, \
{ LOG_NKF13_MSG, sizeof(log_NKF3), \
"NK13","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_NKF14_MSG, sizeof(log_NKF4), \
"NK14","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_NKQ1_MSG, sizeof(log_Quaternion), "NKQ1", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_NKQ2_MSG, sizeof(log_Quaternion), "NKQ2", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_NKQ3_MSG, sizeof(log_Quaternion), "NKQ3", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_NKQ_MSG, sizeof(log_Quaternion), "NKQ", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKF1_MSG, sizeof(log_EKF1), \
"XKF1","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnnnmmmkkkm", "FBBB0000000BBBB" }, \
"XKF1","QBccCfffffffccce","TimeUS,C,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "s#ddhnnnnmmmkkkm", "F-BBB0000000BBBB" }, \
{ LOG_XKF2_MSG, sizeof(log_NKF2a), \
"XKF2","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s---nnGGGGGG-", "F---BBCCCCCC-" }, \
"XKF2","QBccccchhhhhhB","TimeUS,C,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s#---nnGGGGGG-", "F----BBCCCCCC-" }, \
{ LOG_XKF3_MSG, sizeof(log_NKF3), \
"XKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGG??", "FBBBBBBCCCBB" }, \
"XKF3","QBcccccchhhcc","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "s#nnnmmmGGG??", "F-BBBBBBCCCBB" }, \
{ LOG_XKF4_MSG, sizeof(log_NKF4), \
"XKF4","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s------??-----", "F------??-----" }, \
"XKF4","QBcccccfbbHBHHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
{ LOG_XKF5_MSG, sizeof(log_NKF5), \
"XKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s----m???mrnm", "F----BBBBB000" }, \
{ LOG_XKF6_MSG, sizeof(log_EKF1), \
"XKF6","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_XKF7_MSG, sizeof(log_NKF2a), \
"XKF7","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s---nnGGGGGG-", "F---BBCCCCCC-" }, \
{ LOG_XKF8_MSG, sizeof(log_NKF3), \
"XKF8","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_XKF9_MSG, sizeof(log_NKF4), \
"XKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \
"XKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s-m---mmmmmmmm", "F-B---BBBBBBBB" }, \
{ LOG_XKF11_MSG, sizeof(log_EKF1), \
"XK11","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_XKF12_MSG, sizeof(log_NKF2a), \
"XK12","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s---nnGGGGGG-", "F---BBCCCCCC-" }, \
{ LOG_XKF13_MSG, sizeof(log_NKF3), \
"XK13","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_XKF14_MSG, sizeof(log_NKF4), \
"XK14","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_XKQ1_MSG, sizeof(log_Quaternion), "XKQ1", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKQ2_MSG, sizeof(log_Quaternion), "XKQ2", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKQ3_MSG, sizeof(log_Quaternion), "XKQ3", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKQ_MSG, sizeof(log_Quaternion), "XKQ", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKFD_MSG, sizeof(log_ekfBodyOdomDebug), \
"XKFD","Qffffff","TimeUS,IX,IY,IZ,IVX,IVY,IVZ", "s------", "F------" }, \
{ LOG_XKV1_MSG, sizeof(log_ekfStateVar), \
Expand Down Expand Up @@ -1613,35 +1586,15 @@ enum LogMessages : uint8_t {
LOG_NKF3_MSG,
LOG_NKF4_MSG,
LOG_NKF5_MSG,
LOG_NKF6_MSG,
LOG_NKF7_MSG,
LOG_NKF8_MSG,
LOG_NKF9_MSG,
LOG_NKF10_MSG,
LOG_NKF11_MSG,
LOG_NKF12_MSG,
LOG_NKF13_MSG,
LOG_NKF14_MSG,
LOG_NKQ1_MSG,
LOG_NKQ2_MSG,
LOG_NKQ3_MSG,
LOG_NKQ_MSG,
LOG_XKF1_MSG,
LOG_XKF2_MSG,
LOG_XKF3_MSG,
LOG_XKF4_MSG,
LOG_XKF5_MSG,
LOG_XKF6_MSG,
LOG_XKF7_MSG,
LOG_XKF8_MSG,
LOG_XKF9_MSG,
LOG_XKF10_MSG,
LOG_XKF11_MSG,
LOG_XKF12_MSG,
LOG_XKF13_MSG,
LOG_XKF14_MSG,
LOG_XKQ1_MSG,
LOG_XKQ2_MSG,
LOG_XKQ3_MSG,
LOG_XKQ_MSG,
LOG_XKFD_MSG,
LOG_XKV1_MSG,
LOG_XKV2_MSG,
Expand Down
10 changes: 5 additions & 5 deletions libraries/AP_NavEKF2/AP_NavEKF2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -530,11 +530,11 @@ class NavEKF2 {
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_NKF1(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF2(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF3(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF4(uint8_t core, 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_Quaternion(uint8_t core, uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us) const;
};
52 changes: 21 additions & 31 deletions libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h>

void NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
void NavEKF2::Log_Write_NKF1(uint8_t _core, uint64_t time_us) const
{
// Write first EKF packet
Vector3f euler;
Expand All @@ -23,8 +23,9 @@ void NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us
originLLH.alt = 0;
}
const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(msg_id),
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
time_us : time_us,
core : _core,
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)
Expand All @@ -43,7 +44,7 @@ void NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

void NavEKF2::Log_Write_NKF2(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
void NavEKF2::Log_Write_NKF2(uint8_t _core, uint64_t time_us) const
{
// Write second EKF packet
float azbias = 0;
Expand All @@ -58,8 +59,9 @@ void NavEKF2::Log_Write_NKF2(uint8_t _core, LogMessages msg_id, uint64_t time_us
getMagXYZ(_core,magXYZ);
getGyroScaleErrorPercentage(_core,gyroScaleFactor);
const struct log_NKF2 pkt2{
LOG_PACKET_HEADER_INIT(msg_id),
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
time_us : time_us,
core : _core,
AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x),
scaleY : (int16_t)(100*gyroScaleFactor.y),
Expand All @@ -77,7 +79,7 @@ void NavEKF2::Log_Write_NKF2(uint8_t _core, LogMessages msg_id, uint64_t time_us
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}

void NavEKF2::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
void NavEKF2::Log_Write_NKF3(uint8_t _core, uint64_t time_us) const
{
// Write third EKF packet
Vector3f velInnov;
Expand All @@ -87,8 +89,9 @@ void NavEKF2::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us
float yawInnov = 0;
getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov);
const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(msg_id),
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
time_us : time_us,
core : _core,
innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z),
Expand All @@ -104,7 +107,7 @@ void NavEKF2::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us
AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
}

void NavEKF2::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
void NavEKF2::Log_Write_NKF4(uint8_t _core, uint64_t time_us) const
{
// Write fourth EKF packet
float velVar = 0;
Expand All @@ -127,8 +130,9 @@ void NavEKF2::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us
getTiltError(_core,tiltError);
int8_t primaryIndex = getPrimaryCoreIndex();
const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(msg_id),
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
time_us : time_us,
core : _core,
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
Expand Down Expand Up @@ -179,14 +183,15 @@ void NavEKF2::Log_Write_NKF5(uint64_t time_us) const
AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
}

void NavEKF2::Log_Write_Quaternion(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
void NavEKF2::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const
{
// log quaternion
Quaternion quat;
getQuaternion(_core, quat);
const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(msg_id),
LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG),
time_us : time_us,
core : _core,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
Expand Down Expand Up @@ -240,29 +245,14 @@ void NavEKF2::Log_Write()

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 EKF state info for the second EFK core 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);
}

// log EKF state info for the third EFK core if enabled
if (activeCores() >= 3) {
Log_Write_EKF1(2, LOG_NKF11_MSG, time_us);
Log_Write_NKF2(2, LOG_NKF12_MSG, time_us);
Log_Write_NKF3(2, LOG_NKF13_MSG, time_us);
Log_Write_NKF4(2, LOG_NKF14_MSG, time_us);
Log_Write_Quaternion(2, LOG_NKQ3_MSG, time_us);
for (uint8_t i=0; i<activeCores(); i++) {
Log_Write_NKF1(i, time_us);
Log_Write_NKF2(i, time_us);
Log_Write_NKF3(i, time_us);
Log_Write_NKF4(i, time_us);
Log_Write_Quaternion(i, time_us);
}

// write range beacon fusion debug packet if the range value is non-zero
Expand Down
12 changes: 6 additions & 6 deletions libraries/AP_NavEKF3/AP_NavEKF3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -546,12 +546,12 @@ class NavEKF3 {
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_NKF2a(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_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(uint64_t time_us) const;
void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us) const;
void Log_Write_BodyOdom(uint64_t time_us) const;
void Log_Write_State_Variances(uint64_t time_us) const;
Expand Down
Loading