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

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

c415-sdk
Peter Barker 5 years ago committed by Andrew Tridgell
parent
524647d221
commit
03cca68c5a
2 changed files with 29 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      libraries/AP_NavEKF3/AP_NavEKF3.h
  2. 57
      libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

12
libraries/AP_NavEKF3/AP_NavEKF3.h
Unescape View File

@ -544,12 +544,12 @@ 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_XKF1(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF2a(uint8_t core, LogMessages msg_id, uint64_t time_us) const; void Log_Write_XKF2(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_XKF3(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_XKF4(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF5(uint64_t time_us) const; void Log_Write_XKF5(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;
void Log_Write_BodyOdom(uint64_t time_us) const; void Log_Write_BodyOdom(uint64_t time_us) const;
void Log_Write_State_Variances(uint64_t time_us) const; void Log_Write_State_Variances(uint64_t time_us) const;

57
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
Unescape 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 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 // Write first EKF packet
Vector3f euler; Vector3f euler;
@ -23,8 +23,9 @@ void NavEKF3::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_XKF1_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 NavEKF3::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 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 // Write second EKF packet
Vector3f accelBias; Vector3f accelBias;
@ -56,8 +57,9 @@ void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_u
getMagNED(_core,magNED); getMagNED(_core,magNED);
getMagXYZ(_core,magXYZ); getMagXYZ(_core,magXYZ);
const struct log_NKF2a pkt2{ const struct log_NKF2a pkt2{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
time_us : time_us, time_us : time_us,
core : _core,
accBiasX : (int16_t)(100*accelBias.x), accBiasX : (int16_t)(100*accelBias.x),
accBiasY : (int16_t)(100*accelBias.y), accBiasY : (int16_t)(100*accelBias.y),
accBiasZ : (int16_t)(100*accelBias.z), 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)); 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 // Write third EKF packet
Vector3f velInnov; Vector3f velInnov;
@ -84,8 +86,9 @@ void NavEKF3::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_XKF3_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),
@ -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)); 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 // Write fourth EKF packet
float velVar = 0; 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); getTiltError(_core,tiltError);
uint8_t primaryIndex = getPrimaryCoreIndex(); uint8_t primaryIndex = getPrimaryCoreIndex();
const struct log_NKF4 pkt4{ const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(msg_id), LOG_PACKET_HEADER_INIT(LOG_XKF4_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),
@ -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 // 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 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)); 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 // 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_XKQ_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,
@ -304,32 +309,16 @@ void NavEKF3::Log_Write()
uint64_t time_us = AP_HAL::micros64(); uint64_t time_us = AP_HAL::micros64();
Log_Write_EKF1(0, LOG_XKF1_MSG, time_us); Log_Write_XKF5(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 EKF state info for the second EFK core if enabled for (uint8_t i=0; i<activeCores(); i++) {
if (activeCores() >= 2) { Log_Write_XKF1(i, time_us);
Log_Write_EKF1(1, LOG_XKF6_MSG, time_us); Log_Write_XKF2(i, time_us);
Log_Write_NKF2a(1, LOG_XKF7_MSG, time_us); Log_Write_XKF3(i, time_us);
Log_Write_NKF3(1, LOG_XKF8_MSG, time_us); Log_Write_XKF4(i, time_us);
Log_Write_NKF4(1, LOG_XKF9_MSG, time_us); Log_Write_Quaternion(i, time_us);
Log_Write_Quaternion(1, LOG_XKQ2_MSG, 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 // write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us); Log_Write_Beacon(time_us);

Write Preview
Loading…
Cancel
Save