AP_NavEKF2: use instance number when logging rather than multiple message IDs
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
9d7016dca8
commit
524647d221
@ -528,11 +528,11 @@ 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_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;
|
||||
};
|
||||
|
||||
@ -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;
|
||||
@ -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)
|
||||
@ -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;
|
||||
@ -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),
|
||||
@ -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;
|
||||
@ -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),
|
||||
@ -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;
|
||||
@ -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),
|
||||
@ -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,
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user