fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 14:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Compensate optical flow data for sensor position offset

Browse Source
This commit is contained in:
Paul Riseborough
2016-04-12 11:14:31 +10:00
parent 48b105b748
commit b46053415f
5 changed files with 27 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+4 -3
View File
@@ -271,10 +271,11 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
optflow_sample_new.flowRadXY = - flow->flowdata;
optflow_sample_new.gyroXY = - flow->gyrodata;
optflow_sample_new.gyroXYZ = - flow->gyrodata;
// compensate for body motion to give a LOS rate
optflow_sample_new.flowRadXYcomp = optflow_sample_new.flowRadXY - optflow_sample_new.gyroXY;
// convert integraton interval to seconds
optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);
// convert integration interval to seconds
optflow_sample_new.dt = 1e-6f * (float)flow->dt;
_time_last_optflow = time_usec;
// push to buffer