From 03cca68c5a42823a149e4b4af1859a49de95c796 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 6 Dec 2019 18:16:12 +1100 Subject: [PATCH] AP_NavEKF3: use instance number when logging rather than multiple message IDs --- libraries/AP_NavEKF3/AP_NavEKF3.h | 12 ++--- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 57 +++++++++------------ 2 files changed, 29 insertions(+), 40 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 715da8a5e8..77a62fdd16 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -544,12 +544,12 @@ private: 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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 573174526b..a4798e71f1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -3,7 +3,7 @@ #include #include -void NavEKF3::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const { // Write first EKF packet Vector3f euler; @@ -23,8 +23,9 @@ void NavEKF3::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_XKF1_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) @@ -43,7 +44,7 @@ void NavEKF3::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const { // Write second EKF packet Vector3f accelBias; @@ -56,8 +57,9 @@ void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_u getMagNED(_core,magNED); getMagXYZ(_core,magXYZ); const struct log_NKF2a pkt2{ - LOG_PACKET_HEADER_INIT(msg_id), + LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG), time_us : time_us, + core : _core, accBiasX : (int16_t)(100*accelBias.x), accBiasY : (int16_t)(100*accelBias.y), accBiasZ : (int16_t)(100*accelBias.z), @@ -74,7 +76,7 @@ void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_u AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } -void NavEKF3::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const { // Write third EKF packet Vector3f velInnov; @@ -84,8 +86,9 @@ void NavEKF3::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_XKF3_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), @@ -101,7 +104,7 @@ void NavEKF3::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us AP::logger().WriteBlock(&pkt3, sizeof(pkt3)); } -void NavEKF3::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const { // Write fourth EKF packet float velVar = 0; @@ -124,8 +127,9 @@ void NavEKF3::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us getTiltError(_core,tiltError); uint8_t primaryIndex = getPrimaryCoreIndex(); const struct log_NKF4 pkt4{ - LOG_PACKET_HEADER_INIT(msg_id), + LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG), time_us : time_us, + core : _core, sqrtvarV : (int16_t)(100*velVar), sqrtvarP : (int16_t)(100*posVar), sqrtvarH : (int16_t)(100*hgtVar), @@ -144,7 +148,7 @@ void NavEKF3::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us } -void NavEKF3::Log_Write_NKF5(uint64_t time_us) const +void NavEKF3::Log_Write_XKF5(uint64_t time_us) const { // 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 @@ -177,14 +181,15 @@ void NavEKF3::Log_Write_NKF5(uint64_t time_us) const AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); } -void NavEKF3::Log_Write_Quaternion(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +void NavEKF3::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_XKQ_MSG), time_us : time_us, + core : _core, q1 : quat.q1, q2 : quat.q2, q3 : quat.q3, @@ -304,32 +309,16 @@ void NavEKF3::Log_Write() uint64_t time_us = AP_HAL::micros64(); - Log_Write_EKF1(0, LOG_XKF1_MSG, time_us); - Log_Write_NKF2a(0, LOG_XKF2_MSG, time_us); - Log_Write_NKF3(0, LOG_XKF3_MSG, time_us); - Log_Write_NKF4(0, LOG_XKF4_MSG, time_us); - Log_Write_NKF5(time_us); - Log_Write_Quaternion(0, LOG_XKQ1_MSG, time_us); + Log_Write_XKF5(time_us); - // log EKF state info for the second EFK core if enabled - if (activeCores() >= 2) { - Log_Write_EKF1(1, LOG_XKF6_MSG, time_us); - Log_Write_NKF2a(1, LOG_XKF7_MSG, time_us); - Log_Write_NKF3(1, LOG_XKF8_MSG, time_us); - Log_Write_NKF4(1, LOG_XKF9_MSG, time_us); - Log_Write_Quaternion(1, LOG_XKQ2_MSG, time_us); + for (uint8_t i=0; i= 3) { - Log_Write_EKF1(2, LOG_XKF11_MSG, time_us); - Log_Write_NKF2a(2, LOG_XKF12_MSG, time_us); - Log_Write_NKF3(2, LOG_XKF13_MSG, time_us); - Log_Write_NKF4(2, LOG_XKF14_MSG, time_us); - Log_Write_Quaternion(2, LOG_XKQ3_MSG, time_us); - } - - // write range beacon fusion debug packet if the range value is non-zero Log_Write_Beacon(time_us);