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

LPE: Use euler angles derived from quaternion

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-18 20:46:08 +02:00
parent ac936a28dd
commit 13833e5fd6
5 changed files with 34 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+5 -3
View File
@@ -64,7 +64,10 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
return -1;
}
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
matrix::Euler<float> euler(q);
float d = agl() * cosf(euler(0)) * cosf(euler(1));
// optical flow in x, y axis
// TODO consider making flow scale a states of the kalman filter
@@ -98,8 +101,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
0);
// rotation of flow from body to nav frame
Matrix3f R_nb(_sub_att.get().R);
Vector3f delta_n = R_nb * delta_b;
Vector3f delta_n = _R_att * delta_b;
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;