fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:13:34 +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/BlockLocalPositionEstimator.cpp
+4 -3
View File
@@ -25,6 +25,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
_sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// main prediction loop, 100 hz
@@ -663,9 +664,9 @@ void BlockLocalPositionEstimator::publishOdom()
_pub_odom.get().vz = xLP(X_vz); // vel down
// angular velocity
_pub_odom.get().rollspeed = _sub_att.get().rollspeed; // roll rate
_pub_odom.get().pitchspeed = _sub_att.get().pitchspeed; // pitch rate
_pub_odom.get().yawspeed = _sub_att.get().yawspeed; // yaw rate
_pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate
_pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate
_pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+2
View File
@@ -12,6 +12,7 @@
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -249,6 +250,7 @@ private:
uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
uORB::SubscriptionPollable<vehicle_angular_velocity_s> _sub_angular_velocity;
uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;
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();