aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: use instance number when logging rather than multiple message IDs

Browse Source
This commit is contained in:
Peter Barker 2019-12-06 18:14:10 +11:00 committed by Andrew Tridgell
parent 9d7016dca8
commit 524647d221
2 changed files with 26 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -528,11 +528,11 @@ private:
void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary); void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary);
// logging functions shared by cores: // logging functions shared by cores:
void Log_Write_EKF1(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, LogMessages msg_id, uint64_t time_us) const; void Log_Write_NKF2(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF3(uint8_t core, LogMessages msg_id, uint64_t time_us) const; void Log_Write_NKF3(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF4(uint8_t core, LogMessages msg_id, 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_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; void Log_Write_Beacon(uint64_t time_us) const;
}; };

52
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -3,7 +3,7 @@
#include <AP_HAL/HAL.h> #include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.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 // Write first EKF packet
Vector3f euler; Vector3f euler;
@ -23,8 +23,9 @@ void NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us
originLLH.alt = 0; originLLH.alt = 0;
} }
const struct log_EKF1 pkt{ const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
time_us : time_us, time_us : time_us,
core : _core,
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) 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) 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) 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 NavEKF2::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us
AP::logger().WriteBlock(&pkt, sizeof(pkt)); 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 // Write second EKF packet
float azbias = 0; float azbias = 0;
@ -58,8 +59,9 @@ void NavEKF2::Log_Write_NKF2(uint8_t _core, LogMessages msg_id, uint64_t time_us
getMagXYZ(_core,magXYZ); getMagXYZ(_core,magXYZ);
getGyroScaleErrorPercentage(_core,gyroScaleFactor); getGyroScaleErrorPercentage(_core,gyroScaleFactor);
const struct log_NKF2 pkt2{ const struct log_NKF2 pkt2{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
time_us : time_us, time_us : time_us,
core : _core,
AZbias : (int8_t)(100*azbias), AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x), scaleX : (int16_t)(100*gyroScaleFactor.x),
scaleY : (int16_t)(100*gyroScaleFactor.y), scaleY : (int16_t)(100*gyroScaleFactor.y),
@ -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)); 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 // Write third EKF packet
Vector3f velInnov; Vector3f velInnov;
@ -87,8 +89,9 @@ void NavEKF2::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us
float yawInnov = 0; float yawInnov = 0;
getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov); getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov);
const struct log_NKF3 pkt3{ const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
time_us : time_us, time_us : time_us,
core : _core,
innovVN : (int16_t)(100*velInnov.x), innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y), innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z), innovVD : (int16_t)(100*velInnov.z),
@ -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)); 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 // Write fourth EKF packet
float velVar = 0; float velVar = 0;
@ -127,8 +130,9 @@ void NavEKF2::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us
getTiltError(_core,tiltError); getTiltError(_core,tiltError);
int8_t primaryIndex = getPrimaryCoreIndex(); int8_t primaryIndex = getPrimaryCoreIndex();
const struct log_NKF4 pkt4{ const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
time_us : time_us, time_us : time_us,
core : _core,
sqrtvarV : (int16_t)(100*velVar), sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar), sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar), sqrtvarH : (int16_t)(100*hgtVar),
@ -179,14 +183,15 @@ void NavEKF2::Log_Write_NKF5(uint64_t time_us) const
AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); 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 // log quaternion
Quaternion quat; Quaternion quat;
getQuaternion(_core, quat); getQuaternion(_core, quat);
const struct log_Quaternion pktq1{ const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG),
time_us : time_us, time_us : time_us,
core : _core,
q1 : quat.q1, q1 : quat.q1,
q2 : quat.q2, q2 : quat.q2,
q3 : quat.q3, q3 : quat.q3,
@ -240,29 +245,14 @@ void NavEKF2::Log_Write()
const uint64_t time_us = AP_HAL::micros64(); 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_NKF5(time_us);
Log_Write_Quaternion(0, LOG_NKQ1_MSG, time_us);
// log EKF state info for the second EFK core if enabled for (uint8_t i=0; i<activeCores(); i++) {
if (activeCores() >= 2) { Log_Write_NKF1(i, time_us);
Log_Write_EKF1(1, LOG_NKF6_MSG, time_us); Log_Write_NKF2(i, time_us);
Log_Write_NKF2(1, LOG_NKF7_MSG, time_us); Log_Write_NKF3(i, time_us);
Log_Write_NKF3(1, LOG_NKF8_MSG, time_us); Log_Write_NKF4(i, time_us);
Log_Write_NKF4(1, LOG_NKF9_MSG, time_us); Log_Write_Quaternion(i, 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);
} }
// write range beacon fusion debug packet if the range value is non-zero // write range beacon fusion debug packet if the range value is non-zero