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

attitude message cleanup: more cleanup

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 3faaeb06d1
commit a978d61d9a
6 changed files with 20 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/terrain_estimation/terrain_estimator.cpp
+4 -5
View File
@@ -66,8 +66,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
const struct sensor_combined_s *sensor,
const struct distance_sensor_s *distance)
{
matrix::Quaternion<float> q(&attitude->q[0]);
matrix::Dcm<float> R_att(q);
matrix::Dcmf R_att = matrix::Quatf(attitude->q);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
matrix::Vector<float, 3> u;
u = R_att * a;
@@ -112,8 +111,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
}
if (distance->timestamp > _time_last_distance) {
matrix::Quaternion<float> q(&attitude->q[0]);
matrix::Euler<float> euler(q);
matrix::Quatf q(attitude->q);
matrix::Eulerf euler(q);
float d = distance->current_distance;
matrix::Matrix<float, 1, n_x> C;
@@ -122,7 +121,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
float R = 0.009f;
matrix::Vector<float, 1> y;
y(0) = d * cosf(euler(0)) * cosf(euler(1));
y(0) = d * cosf(euler.phi()) * cosf(euler.theta());
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());