Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+1 -1
View File
@@ -43,7 +43,7 @@ void BlockLocalPositionEstimator::flowDeinit()
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y) int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{ {
// check for sane pitch/roll // 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; return -1;
} }
src/modules/local_position_estimator/sensors/lidar.cpp
+4 -4
View File
@@ -53,8 +53,8 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
_time_last_lidar = _timeStamp; _time_last_lidar = _timeStamp;
y.setZero(); y.setZero();
y(0) = (d + _lidar_z_offset.get()) * y(0) = (d + _lidar_z_offset.get()) *
cosf(_sub_att.get().roll) * cosf(_eul(0)) *
cosf(_sub_att.get().pitch); cosf(_eul(1));
return OK; return OK;
} }
@@ -67,8 +67,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
// account for leaning // account for leaning
y(0) = y(0) * y(0) = y(0) *
cosf(_eul.phi()) * cosf(_eul(0)) *
cosf(_eul.theta()); cosf(_eul(1));
// measurement matrix // measurement matrix
Matrix<float, n_y_lidar, n_x> C; Matrix<float, n_y_lidar, n_x> C;