Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: fixed gaps in EKF logging 

timestamps should not be static as otherwise some lanes will not be
logged
This commit is contained in:
Andrew Tridgell 2021-12-27 09:07:02 +11:00 committed by Randy Mackay
parent b1da3467c4
commit 288f4bfbb4
3 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -273,7 +273,6 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
return; return;
} }
static uint32_t lastUpdateTime_ms = 0;
const uint32_t updateTime_ms = MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms); const uint32_t updateTime_ms = MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
if (updateTime_ms > lastUpdateTime_ms) { if (updateTime_ms > lastUpdateTime_ms) {
const struct log_XKFD pkt11{ const struct log_XKFD pkt11{
@ -293,14 +292,13 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
} }
#endif #endif
void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us)
{ {
if (core_index != frontend->primary) { if (core_index != frontend->primary) {
// log only primary instance for now // log only primary instance for now
return; return;
} }
static uint32_t lastEkfStateVarLogTime_ms = 0;
if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) { if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) {
lastEkfStateVarLogTime_ms = AP::dal().millis(); lastEkfStateVarLogTime_ms = AP::dal().millis();
const struct log_XKV pktv1{ const struct log_XKV pktv1{
@ -394,7 +392,6 @@ void NavEKF3_core::Log_Write(uint64_t time_us)
void NavEKF3_core::Log_Write_Timing(uint64_t time_us) void NavEKF3_core::Log_Write_Timing(uint64_t time_us)
{ {
// log EKF timing statistics every 5s // log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
if (AP::dal().millis() - lastTimingLogTime_ms <= 5000) { if (AP::dal().millis() - lastTimingLogTime_ms <= 5000) {
return; return;
} }

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -2182,7 +2182,6 @@ void NavEKF3_core::verifyTiltErrorVariance()
} }
tiltErrorVarianceAlt = MIN(tiltErrorVarianceAlt, sq(radians(30.0f))); tiltErrorVarianceAlt = MIN(tiltErrorVarianceAlt, sq(radians(30.0f)));
static uint32_t lastLogTime_ms = 0;
if (imuSampleTime_ms - lastLogTime_ms > 500) { if (imuSampleTime_ms - lastLogTime_ms > 500) {
lastLogTime_ms = imuSampleTime_ms; lastLogTime_ms = imuSampleTime_ms;
const struct log_XKTV msg { const struct log_XKTV msg {

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1479,6 +1479,12 @@ private:
bool EKFGSF_run_filterbank; // true when the filter bank is active bool EKFGSF_run_filterbank; // true when the filter bank is active
uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate
// logging timestamps
uint32_t lastLogTime_ms;
uint32_t lastUpdateTime_ms;
uint32_t lastEkfStateVarLogTime_ms;
uint32_t lastTimingLogTime_ms;
// bits in EK3_AFFINITY // bits in EK3_AFFINITY
enum ekf_affinity { enum ekf_affinity {
EKF_AFFINITY_GPS = (1U<<0), EKF_AFFINITY_GPS = (1U<<0),
@ -1519,7 +1525,7 @@ private:
void Log_Write_Quaternion(uint64_t time_us) const; void Log_Write_Quaternion(uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us); void Log_Write_Beacon(uint64_t time_us);
void Log_Write_BodyOdom(uint64_t time_us); void Log_Write_BodyOdom(uint64_t time_us);
void Log_Write_State_Variances(uint64_t time_us) const; void Log_Write_State_Variances(uint64_t time_us);
void Log_Write_Timing(uint64_t time_us); void Log_Write_Timing(uint64_t time_us);
void Log_Write_GSF(uint64_t time_us); void Log_Write_GSF(uint64_t time_us);
}; };