Skip to content

Commit

Permalink
AP_NavEKF3: use instance number when logging rather than multiple mes…
Browse files Browse the repository at this point in the history 
…sage IDs
  • Loading branch information
@peterbarker @tridge
peterbarker authored and tridge committed Dec 17, 2019
1 parent 524647d commit 03cca68
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 40 deletions.
12 changes: 6 additions & 6 deletions libraries/AP_NavEKF3/AP_NavEKF3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -544,12 +544,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
57 changes: 23 additions & 34 deletions libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View file
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 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;
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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),
Expand All @@ -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;
Expand All @@ -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),
Expand All @@ -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;
Expand All @@ -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),
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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);

Expand Down

0 comments on commit 03cca68

Please sign in to comment.