|
|
|
@ -163,7 +163,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
@@ -163,7 +163,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
|
|
|
|
|
// 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; |
|
|
|
|
ofDataNew.body_offset = posOffset; |
|
|
|
|
// write flow rate measurements corrected for body rates
|
|
|
|
|
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; |
|
|
|
|
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; |
|
|
|
|