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

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/terrain_estimation/terrain_estimator.cpp
+7 -5
View File
@@ -65,9 +65,10 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
const struct sensor_combined_s *sensor,
const struct distance_sensor_s *distance)
{
if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
if (attitude->q_valid) {
matrix::Quaternion<float> q(&attitude->q[0]);
matrix::Dcm<float> R_att(q);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
matrix::Vector<float, 3> u;
u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity
@@ -115,7 +116,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);
float d = distance->current_distance;
matrix::Matrix<float, 1, n_x> C;
@@ -124,7 +126,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(attitude->roll) * cosf(attitude->pitch);
y(0) = d * cosf(euler(0)) * cosf(euler(1));
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());