zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

c415-sdk
Peter Barker 5 years ago committed by Andrew Tridgell
parent
9d7016dca8
commit
524647d221
2 changed files with 26 additions and 36 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AP_NavEKF2/AP_NavEKF2.h
  2. 52
      libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp

10
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -528,11 +528,11 @@ private: @@ -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;
};

52
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
Unescape View File

@ -3,7 +3,7 @@ @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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() @@ -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

Write Preview
Loading…
Cancel
Save