fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

sih update orb usage

Browse Source
This commit is contained in:
Daniel Agar
2019-11-30 15:52:53 -05:00
parent c1c9895462
commit b418f937a3
2 changed files with 17 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/sih.cpp
+6 -40
View File
@@ -46,8 +46,6 @@
#include <px4_platform_common/log.h>
#include <drivers/drv_pwm_output.h> // to get PWM flags
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h> // to get the HIL status
#include <unistd.h>
#include <string.h>
@@ -106,9 +104,6 @@ Sih::Sih() :
void Sih::run()
{
// to subscribe to (read) the actuators_out pwm
_actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
// initialize parameters
parameters_update_poll();
@@ -141,7 +136,6 @@ void Sih::run()
hrt_cancel(&_timer_call); // close the periodic timer interruption
px4_sem_destroy(&_data_semaphore);
orb_unsubscribe(_actuator_out_sub);
}
// timer_callback() is used as a real time callback to post the semaphore
@@ -255,15 +249,9 @@ void Sih::init_sensors()
// read the motor signals outputted from the mixer
void Sih::read_motors()
{
actuator_outputs_s actuators_out {};
// read the actuator outputs
bool updated = false;
orb_check(_actuator_out_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out);
actuator_outputs_s actuators_out;
if (_actuator_out_sub.update(&actuators_out)) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
_u[i] = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
}
@@ -396,12 +384,7 @@ void Sih::send_gps()
_vehicle_gps_pos.cog_rad = atan2(_gps_vel(1),
_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
if (_vehicle_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos);
} else {
_vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos);
}
_vehicle_gps_pos_pub.publish(_vehicle_gps_pos);
}
void Sih::publish_sih()
@@ -412,14 +395,7 @@ void Sih::publish_sih()
_vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed;
_vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed;
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 {
_vehicle_angular_velocity_gt_pub = orb_advertise(ORB_ID(vehicle_angular_velocity_groundtruth),
&_vehicle_angular_velocity_gt);
}
_vehicle_angular_velocity_gt_pub.publish(_vehicle_angular_velocity_gt);
// publish attitude groundtruth
_att_gt.timestamp = hrt_absolute_time();
@@ -428,12 +404,7 @@ void Sih::publish_sih()
_att_gt.q[2] = _q(2);
_att_gt.q[3] = _q(3);
if (_att_gt_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_pub, &_att_gt);
} else {
_att_gt_pub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
}
_att_gt_pub.publish(_att_gt);
_gpos_gt.timestamp = hrt_absolute_time();
_gpos_gt.lat = _gps_lat_noiseless;
@@ -443,12 +414,7 @@ void Sih::publish_sih()
_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);
}
_gpos_gt_pub.publish(_gpos_gt);
}
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
src/modules/sih/sih.hpp
+11 -9
View File
@@ -46,8 +46,10 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
#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
@@ -105,23 +107,23 @@ private:
PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
// to publish the gps position
vehicle_gps_position_s _vehicle_gps_pos{};
orb_advert_t _vehicle_gps_pos_pub{nullptr};
vehicle_gps_position_s _vehicle_gps_pos{};
uORB::Publication<vehicle_gps_position_s> _vehicle_gps_pos_pub{ORB_ID(vehicle_gps_position)};
// angular velocity groundtruth
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
orb_advert_t _vehicle_angular_velocity_gt_pub{nullptr};
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
// attitude groundtruth
vehicle_attitude_s _att_gt{};
orb_advert_t _att_gt_pub{nullptr};
vehicle_attitude_s _att_gt{};
uORB::Publication<vehicle_attitude_s> _att_gt_pub{ORB_ID(vehicle_attitude_groundtruth)};
// global position groundtruth
vehicle_global_position_s _gpos_gt{};
orb_advert_t _gpos_gt_pub{nullptr};
vehicle_global_position_s _gpos_gt{};
uORB::Publication<vehicle_global_position_s> _gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
int _actuator_out_sub {-1};
uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
// hard constants
static constexpr uint16_t NB_MOTORS = 4;