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

DataFlash : EKF optical flow fusion support preliminary changes

Browse Source
This commit is contained in:
priseborough 2014-08-07 10:32:12 +10:00 committed by Andrew Tridgell
parent 7b76fc29fb
commit bc74abcd00
2 changed files with 36 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/DataFlash/DataFlash.h
View File

@ -347,6 +347,18 @@ struct PACKED log_EKF4 {
uint8_t staticmode;
};
struct PACKED log_EKF5 {
LOG_PACKET_HEADER;
uint32_t time_ms;
float obsX;
float obsY;
float innovX;
float innovY;
float gndPos;
uint8_t scaler;
uint8_t quality;
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint32_t time_ms;
@ -514,7 +526,9 @@ struct PACKED log_Esc {
{ LOG_ESC7_MSG, sizeof(log_Esc), \
"ESC7", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
{ LOG_ESC8_MSG, sizeof(log_Esc), \
"ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }
"ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
{ LOG_EKF5_MSG, sizeof(log_EKF5), \
"EKF5","IfffffBB","TimeMS,FX,FY,IFX,IFY,GP,SF,Q" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@ -559,6 +573,7 @@ struct PACKED log_Esc {
#define LOG_ESC6_MSG 159
#define LOG_ESC7_MSG 160
#define LOG_ESC8_MSG 161
#define LOG_EKF5_MSG 162
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use

20
libraries/DataFlash/LogFile.cpp
View File

@ -992,6 +992,26 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
staticmode : (uint8_t)(ahrs.get_NavEKF().getStaticMode())
};
WriteBlock(&pkt4, sizeof(pkt4));
// Write fifth EKF packet
float fscale;
float gndPos;
float obsX, obsY;
float innovX, innovY;
uint8_t flowQual;
ahrs.get_NavEKF().getFlowDebug(fscale, obsX, obsY, innovX, innovY, gndPos, flowQual);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
time_ms : hal.scheduler->millis(),
obsX : (float)(obsX),
obsY : (float)(obsY),
innovX : (float)(innovX),
innovY : (float)(innovY),
gndPos : (float)(gndPos),
scaler: (uint8_t)(100*fscale),
quality : (uint8_t)(flowQual)
};
WriteBlock(&pkt5, sizeof(pkt5));
}
#endif