AP_NavEKF2: Add flow sensor body position offset to data buffer

This commit is contained in:
priseborough 2016-10-12 08:28:58 +11:00 committed by Andrew Tridgell
parent 92c086b40e
commit 397033b7c3
2 changed files with 3 additions and 0 deletions

View File

@ -142,6 +142,8 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
// note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame
ofDataNew.body_offset = posOffset;
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;

View File

@ -402,6 +402,7 @@ private:
Vector2f flowRadXY; // 0..1
Vector2f flowRadXYcomp; // 2..3
uint32_t time_ms; // 4
Vector3f body_offset; // 5..7
};
// update the navigation filter status