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

Remove q_valid flag from attitude topic

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-18 20:46:08 +02:00
parent d349bd570f
commit 526fb8f515
11 changed files with 31 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/terrain_estimation/terrain_estimator.cpp
+7 -11
View File
@@ -37,6 +37,7 @@
*/
#include "terrain_estimator.h"
#include <geo/geo.h>
#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead
@@ -65,17 +66,12 @@ 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->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
} else {
_u_z = 0.0f;
}
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) + CONSTANTS_ONE_G; // compensate for gravity
// dynamics matrix
matrix::Matrix<float, n_x, n_x> A;