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

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

Browse Source
This commit is contained in:
Peter Barker 2019-12-06 18:16:12 +11:00 committed by Andrew Tridgell
parent 524647d221
commit 03cca68c5a
2 changed files with 29 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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;

57
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -3,7 +3,7 @@
#include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h>
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<activeCores(); i++) {
Log_Write_XKF1(i, time_us);
Log_Write_XKF2(i, time_us);
Log_Write_XKF3(i, time_us);
Log_Write_XKF4(i, time_us);
Log_Write_Quaternion(i, time_us);
}
// log EKF state info for the third EFK core if enabled
if (activeCores() >= 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);