Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Don't log EKF optical flow data if not required

Browse Source
This commit is contained in:
priseborough 2014-12-24 07:39:23 +11:00 committed by Randy Mackay
parent b6d300db19
commit b99b3c7b9d
2 changed files with 25 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/DataFlash/DataFlash.h
View File

@ -69,7 +69,7 @@ public:
void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
#if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
#endif
void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
void Log_Write_Radio(const mavlink_radio_t &packet);

45
libraries/DataFlash/LogFile.cpp
View File

@ -894,7 +894,7 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
}
#if AP_AHRS_NAVEKF_AVAILABLE
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{
// Write first EKF packet
Vector3f euler;
@ -1005,27 +1005,30 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
};
WriteBlock(&pkt4, sizeof(pkt4));
// Write fifth EKF packet
float fscale;
float gndPos;
float flowInnovX, flowInnovY;
float augFlowInnovX, augFlowInnovY;
float rngInnov;
float range;
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
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),
scaler: (uint8_t)(100*fscale),
RI : (int16_t)(100*rngInnov),
range : (uint16_t)(100*range)
};
WriteBlock(&pkt5, sizeof(pkt5));
if (optFlowEnabled) {
float fscale;
float gndPos;
float flowInnovX, flowInnovY;
float augFlowInnovX, augFlowInnovY;
float rngInnov;
float range;
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
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),
scaler: (uint8_t)(100*fscale),
RI : (int16_t)(100*rngInnov),
range : (uint16_t)(100*range)
};
WriteBlock(&pkt5, sizeof(pkt5));
}
}
#endif