fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:33: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/sih/sih.cpp
+28 -16
View File
@@ -408,19 +408,19 @@ void Sih::send_gps()
void Sih::publish_sih()
{
_gpos_gt.timestamp = hrt_absolute_time();
_gpos_gt.lat = _gps_lat_noiseless;
_gpos_gt.lon = _gps_lon_noiseless;
_gpos_gt.alt = _gps_alt_noiseless;
_gpos_gt.vel_n = _v_I(0);
_gpos_gt.vel_e = _v_I(1);
_gpos_gt.vel_d = _v_I(2);
// publish angular velocity groundtruth
_vehicle_angular_velocity_gt.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed;
_vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed;
_vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed;
if (_gpos_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt);
if (_vehicle_angular_velocity_gt_pub != nullptr) {
orb_publish(ORB_ID(vehicle_angular_velocity_groundtruth), _vehicle_angular_velocity_gt_pub,
&_vehicle_angular_velocity_gt);
} else {
_gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
_vehicle_angular_velocity_gt_pub = orb_advertise(ORB_ID(vehicle_angular_velocity_groundtruth),
&_vehicle_angular_velocity_gt);
}
// publish attitude groundtruth
@@ -429,15 +429,27 @@ void Sih::publish_sih()
_att_gt.q[1] = _q(1);
_att_gt.q[2] = _q(2);
_att_gt.q[3] = _q(3);
_att_gt.rollspeed = _w_B(0);
_att_gt.pitchspeed = _w_B(1);
_att_gt.yawspeed = _w_B(2);
if (_att_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt);
if (_att_gt_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_pub, &_att_gt);
} else {
_att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
_att_gt_pub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
}
_gpos_gt.timestamp = hrt_absolute_time();
_gpos_gt.lat = _gps_lat_noiseless;
_gpos_gt.lon = _gps_lon_noiseless;
_gpos_gt.alt = _gps_alt_noiseless;
_gpos_gt.vel_n = _v_I(0);
_gpos_gt.vel_e = _v_I(1);
_gpos_gt.vel_d = _v_I(2);
if (_gpos_gt_pub != nullptr) {
orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_pub, &_gpos_gt);
} else {
_gpos_gt_pub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
}
}
src/modules/sih/sih.hpp
+14 -7
View File
@@ -47,8 +47,9 @@
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <perf/perf_counter.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/vehicle_gps_position.h>
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
@@ -103,14 +104,20 @@ private:
PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
// to publish the gps position
struct vehicle_gps_position_s _vehicle_gps_pos {};
orb_advert_t _vehicle_gps_pos_pub{nullptr};
vehicle_gps_position_s _vehicle_gps_pos{};
orb_advert_t _vehicle_gps_pos_pub{nullptr};
// angular velocity groundtruth
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
orb_advert_t _vehicle_angular_velocity_gt_pub{nullptr};
// attitude groundtruth
struct vehicle_global_position_s _gpos_gt {};
orb_advert_t _gpos_gt_sub{nullptr};
vehicle_attitude_s _att_gt{};
orb_advert_t _att_gt_pub{nullptr};
// global position groundtruth
struct vehicle_attitude_s _att_gt {};
orb_advert_t _att_gt_sub{nullptr};
vehicle_global_position_s _gpos_gt{};
orb_advert_t _gpos_gt_pub{nullptr};
int _parameter_update_sub {-1};
int _actuator_out_sub {-1};