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

lpe: remove usage of euler angles from attitude topic

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-10-09 15:01:42 +02:00 committed by Lorenz Meier
parent 06931e12cf
commit 7c2ebd96a0
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/local_position_estimator/sensors/flow.cpp
View File

@ -43,7 +43,7 @@ void BlockLocalPositionEstimator::flowDeinit()
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for sane pitch/roll
if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
if (_eul(0) > 0.5f || _eul(1) > 0.5f) {
return -1;
}

8
src/modules/local_position_estimator/sensors/lidar.cpp
View File

@ -53,8 +53,8 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
_time_last_lidar = _timeStamp;
y.setZero();
y(0) = (d + _lidar_z_offset.get()) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
cosf(_eul(0)) *
cosf(_eul(1));
return OK;
}
@ -67,8 +67,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
// account for leaning
y(0) = y(0) *
cosf(_eul.phi()) *
cosf(_eul.theta());
cosf(_eul(0)) *
cosf(_eul(1));
// measurement matrix
Matrix<float, n_y_lidar, n_x> C;