Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Add streaming log verbosity level parameter

Browse Source
This commit is contained in:
Henry Wurzburg 2022-07-27 19:56:33 -05:00 committed by Andrew Tridgell
parent 022987e4d3
commit b7f49db1f7
3 changed files with 31 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -720,6 +720,14 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("PRIMARY", 8, NavEKF3, _primary_core, EK3_PRIMARY_DEFAULT),
// @Param: LOG_LEVEL
// @DisplayName: Logging Level
// @Description: Determines how verbose the EKF3 streaming logging is. A value of 0 provides full logging(default), a value of 1 only XKF4 scaled innovations are logged, a value of 2 both XKF4 and GSF are logged, and a value of 3 disables all streaming logging of EKF3.
// @Range: 0 3
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOG_LEVEL", 9, NavEKF3, _log_level, 0),
AP_GROUPEND
};

9
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -359,6 +359,14 @@ private:
uint32_t _frameTimeUsec; // time per IMU frame
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
// values for EK3_LOG_LEVEL
enum class LogLevel {
ALL = 0,
XKF4 = 1,
XKF4_GSF = 2,
NONE = 3
};
// EKF Mavlink Tuneable Parameters
AP_Int8 _enable; // zero to disable EKF3
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
@ -424,6 +432,7 @@ private:
AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test
AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m)
AP_Int8 _primary_core; // initial core number
AP_Enum<LogLevel> _log_level; // log verbosity level
// Possible values for _flowUse
#define FLOW_USE_NONE 0

15
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -365,17 +365,28 @@ void NavEKF3::Log_Write()
void NavEKF3_core::Log_Write(uint64_t time_us)
{
const auto level = frontend->_log_level;
if (level == NavEKF3::LogLevel::NONE) { // no logging from EK3_LOG_LEVEL param
return;
}
Log_Write_XKF4(time_us);
if (level == NavEKF3::LogLevel::XKF4) { // only log XKF4 scaled innovations
return;
}
Log_Write_GSF(time_us);
if (level == NavEKF3::LogLevel::XKF4_GSF) { // only log XKF4 scaled innovations and GSF, otherwise log everything
return;
}
// note that several of these functions exit-early if they're not
// attempting to log the primary core.
Log_Write_XKF1(time_us);
Log_Write_XKF2(time_us);
Log_Write_XKF3(time_us);
Log_Write_XKF4(time_us);
Log_Write_XKF5(time_us);
Log_Write_XKFS(time_us);
Log_Write_Quaternion(time_us);
Log_Write_GSF(time_us);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);