Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash : Add range measurement to EKF debug message

Browse Source
This commit is contained in:
priseborough 2014-10-16 05:56:20 +11:00 committed by Andrew Tridgell
parent 744c72d40b
commit f358d5e20f
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/DataFlash/DataFlash.h
View File

@ -357,6 +357,7 @@ struct PACKED log_EKF5 {
float gndPos;
uint8_t scaler;
uint8_t quality;
uint16_t range;
};
struct PACKED log_Cmd {
@ -528,7 +529,7 @@ struct PACKED log_Esc {
{ LOG_ESC8_MSG, sizeof(log_Esc), \
"ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
{ LOG_EKF5_MSG, sizeof(log_EKF5), \
"EKF5","IfffffBB","TimeMS,flowX,flowY,omegaX,omegaY,gndPos,fScaler,flowQual" }
"EKF5","IfffffBBc","TimeMS,flowX,flowY,omegaX,omegaY,gndPos,fScaler,flowQual,rng" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES

6
libraries/DataFlash/LogFile.cpp
View File

@ -998,8 +998,9 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
float gndPos;
float flowX, flowY;
float omegaX, omegaY;
float range;
uint8_t flowQual;
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowX, flowY, omegaX, omegaY, flowQual);
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowX, flowY, omegaX, omegaY, flowQual, range);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
time_ms : hal.scheduler->millis(),
@ -1009,7 +1010,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
omegaY : (float)(omegaY),
gndPos : (float)(gndPos),
scaler: (uint8_t)(100*fscale),
quality : (uint8_t)(flowQual)
quality : (uint8_t)(flowQual),
range : (uint16_t)(100*range)
};
WriteBlock(&pkt5, sizeof(pkt5));
}