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

lpe: add vehicle_odometry and data validation handlers; improve inout interface

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 7303005373
commit 048ff56890
8 changed files with 260 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+5 -4
View File
@@ -36,8 +36,10 @@ void BlockLocalPositionEstimator::flowInit()
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
// check for sane pitch/roll
if (_eul(0) > 0.5f || _eul(1) > 0.5f) {
if (euler.phi() > 0.5f || euler.theta() > 0.5f) {
return -1;
}
@@ -58,8 +60,6 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
return -1;
}
matrix::Eulerf euler = matrix::Quatf(_sub_att.get().q);
float d = agl() * cosf(euler.phi()) * cosf(euler.theta());
// optical flow in x, y axis
@@ -159,7 +159,8 @@ void BlockLocalPositionEstimator::flowCorrect()
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed;
float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1);
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
_flow_r.get() * _flow_r.get() * rot_sq +