From b418f937a3804948b1c5d73bf4253c778a8d5e14 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 Nov 2019 12:39:10 -0500 Subject: [PATCH] sih update orb usage --- src/modules/sih/sih.cpp | 46 ++++++----------------------------------- src/modules/sih/sih.hpp | 20 ++++++++++-------- 2 files changed, 17 insertions(+), 49 deletions(-) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index dbe0fcb2f9..60bcbb7e73 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -46,8 +46,6 @@ #include #include // to get PWM flags -#include -#include // to get the HIL status #include #include @@ -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 diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 45ea15c51f..f245315a38 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -46,8 +46,10 @@ #include #include #include +#include #include #include +#include #include // to publish groundtruth #include // to publish groundtruth #include // 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_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_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 _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 _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;