Browse Source

DataFlash: Log EKF solution status message

This message is a bitmasked integer that will be used by control software to determine what data is available from the EKF and decide what control modes are available.
Duplicate static mode message removed. Static mode is now contained in the bitmasked solution status message
mission-4.1.18
priseborough 10 years ago committed by Randy Mackay
parent
commit
685fa383e4
  1. 4
      libraries/DataFlash/DataFlash.h
  2. 8
      libraries/DataFlash/LogFile.cpp

4
libraries/DataFlash/DataFlash.h

@ -345,8 +345,8 @@ struct PACKED log_EKF4 { @@ -345,8 +345,8 @@ struct PACKED log_EKF4 {
int8_t offsetNorth;
int8_t offsetEast;
uint8_t faults;
uint8_t staticmode;
uint8_t timeouts;
uint8_t solution;
};
struct PACKED log_EKF5 {
@ -505,7 +505,7 @@ struct PACKED log_Esc { @@ -505,7 +505,7 @@ struct PACKED log_Esc {
{ LOG_EKF3_MSG, sizeof(log_EKF3), \
"EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","IcccccccbbBBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,StaticMode,TS" }, \
"EKF4","IcccccccbbBBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","IBLLHffHH","TimeMS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_UBX1_MSG, sizeof(log_Ubx1), \

8
libraries/DataFlash/LogFile.cpp

@ -982,11 +982,11 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) @@ -982,11 +982,11 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
Vector3f magVar;
float tasVar;
Vector2f offset;
uint8_t faultStatus;
uint8_t timeoutStatus;
uint8_t faultStatus, timeoutStatus, solutionStatus;
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF().getFilterFaults(faultStatus);
ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF().getFilterStatus(solutionStatus);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
time_ms : hal.scheduler->millis(),
@ -1000,8 +1000,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) @@ -1000,8 +1000,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y),
faults : (uint8_t)(faultStatus),
staticmode : (uint8_t)(ahrs.get_NavEKF().getStaticMode()),
timeouts : (uint8_t)(timeoutStatus)
timeouts : (uint8_t)(timeoutStatus),
solution : (uint8_t)(solutionStatus)
};
WriteBlock(&pkt4, sizeof(pkt4));

Loading…
Cancel
Save