Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
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 // 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 // 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) 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 // write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x; ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

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