Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Update EKF optical flow data logging

Browse Source
This commit is contained in:
priseborough 2015-01-03 13:50:47 +11:00 committed by Andrew Tridgell
parent 69e86d3ea4
commit a9eec29e45
2 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/DataFlash/DataFlash.h
View File

@ -354,12 +354,13 @@ struct PACKED log_EKF5 {
uint32_t time_ms;
int16_t FIX;
int16_t FIY;
int16_t AFIX;
int16_t AFIY;
int16_t gndPos;
uint8_t normInnovFX;
uint8_t normInnovFY;
uint16_t estHAGL;
uint8_t scaler;
int16_t RI;
uint16_t range;
uint16_t meaRng;
uint16_t errHAGL;
};
struct PACKED log_Cmd {
@ -531,7 +532,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","IhhhhcBcC","TimeMS,FIX,FIY,AFIX,AFIY,gndPos,fScaler,RI,rng" }
"EKF5","IhhBBCBcCC","TimeMS,FIX,FIY,SFX,SFY,estHAGL,fScaler,RI,meaRng,errHAGL" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES

16
libraries/DataFlash/LogFile.cpp
View File

@ -1009,23 +1009,25 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
// Write fifth EKF packet
if (optFlowEnabled) {
float fscale;
float gndPos;
float estHAGL;
float flowInnovX, flowInnovY;
float augFlowInnovX, augFlowInnovY;
float flowVarX, flowVarY;
float rngInnov;
float range;
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
float gndOffsetErr;
ahrs.get_NavEKF().getFlowDebug(fscale, estHAGL, flowInnovX, flowInnovY, flowVarX, flowVarY, rngInnov, range, gndOffsetErr);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
time_ms : hal.scheduler->millis(),
FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY),
AFIX : (int16_t)(1000*augFlowInnovX),
AFIY : (int16_t)(1000*augFlowInnovY),
gndPos : (int16_t)(100*gndPos),
normInnovFX : min((uint8_t)(100*flowVarX),255),
normInnovFY : min((uint8_t)(100*flowVarY),255),
estHAGL : (uint16_t)(100*estHAGL),
scaler: (uint8_t)(100*fscale),
RI : (int16_t)(100*rngInnov),
range : (uint16_t)(100*range)
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr)
};
WriteBlock(&pkt5, sizeof(pkt5));
}