Copter: Add body position offset to optical flow interface

This commit is contained in:
priseborough 2016-10-12 08:24:25 +11:00 committed by Andrew Tridgell
parent fd905c23e1
commit 209e364190
1 changed files with 2 additions and 1 deletions

View File

@ -138,7 +138,8 @@ void Copter::update_optical_flow(void)
uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
Vector3f posOffset = optflow.get_pos_offset();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}