Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -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_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs); void Log_Write_AHRS2(AP_AHRS &ahrs);
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs); void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
#endif #endif
void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd); void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
void Log_Write_Radio(const mavlink_radio_t &packet); 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 #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 // Write first EKF packet
Vector3f euler; Vector3f euler;
@ -1005,27 +1005,30 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
}; };
WriteBlock(&pkt4, sizeof(pkt4)); WriteBlock(&pkt4, sizeof(pkt4));
// Write fifth EKF packet // Write fifth EKF packet
float fscale; if (optFlowEnabled) {
float gndPos; float fscale;
float flowInnovX, flowInnovY; float gndPos;
float augFlowInnovX, augFlowInnovY; float flowInnovX, flowInnovY;
float rngInnov; float augFlowInnovX, augFlowInnovY;
float range; float rngInnov;
ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range); float range;
struct log_EKF5 pkt5 = { ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, augFlowInnovX, augFlowInnovY, rngInnov, range);
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG), struct log_EKF5 pkt5 = {
time_ms : hal.scheduler->millis(), LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
FIX : (int16_t)(1000*flowInnovX), time_ms : hal.scheduler->millis(),
FIY : (int16_t)(1000*flowInnovY), FIX : (int16_t)(1000*flowInnovX),
AFIX : (int16_t)(1000*augFlowInnovX), FIY : (int16_t)(1000*flowInnovY),
AFIY : (int16_t)(1000*augFlowInnovY), AFIX : (int16_t)(1000*augFlowInnovX),
gndPos : (int16_t)(100*gndPos), AFIY : (int16_t)(1000*augFlowInnovY),
scaler: (uint8_t)(100*fscale), gndPos : (int16_t)(100*gndPos),
RI : (int16_t)(100*rngInnov), scaler: (uint8_t)(100*fscale),
range : (uint16_t)(100*range) RI : (int16_t)(100*rngInnov),
}; range : (uint16_t)(100*range)
WriteBlock(&pkt5, sizeof(pkt5)); };
WriteBlock(&pkt5, sizeof(pkt5));
}
} }
#endif #endif