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

sensors: create vehicle_angular_velocity module (#12596)

Browse Source 
* split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl
This commit is contained in:
Daniel Agar
2019-08-06 12:55:25 -04:00
committed by GitHub
parent bf0eaf4d54
commit 2ad12d7977
34 changed files with 784 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+4 -3
View File
@@ -152,9 +152,10 @@ void BlockLocalPositionEstimator::flowCorrect()
// compute polynomial value
float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed;
const Vector3f rates{_sub_angular_velocity.get().xyz};
float rotrate_sq = rates(0) * rates(0)
+ rates(1) * rates(1)
+ rates(2) * rates(2);
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();