From 528d2f61a0ac30daa100f06de0cc78dcc072e43e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jun 2019 14:40:16 -0400 Subject: [PATCH] sensors partially move to new uORB::Subscription --- src/modules/sensors/rc_update.cpp | 49 +++----------------- src/modules/sensors/rc_update.h | 23 +++------- src/modules/sensors/sensors.cpp | 74 +++++++------------------------ 3 files changed, 28 insertions(+), 118 deletions(-) diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 05479c38c2..8d129c0853 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -39,14 +39,8 @@ #include "rc_update.h" -#include -#include -#include - -#include #include #include -#include using namespace sensors; @@ -57,32 +51,6 @@ RCUpdate::RCUpdate(const Parameters ¶meters) _filter_yaw(50.0f, 10.f), _filter_throttle(50.0f, 10.f) { - memset(&_rc, 0, sizeof(_rc)); - memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); - memset(&_param_rc_values, 0, sizeof(_param_rc_values)); -} - -int RCUpdate::init() -{ - _rc_sub = orb_subscribe(ORB_ID(input_rc)); - - if (_rc_sub < 0) { - return -errno; - } - - _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); - - if (_rc_parameter_map_sub < 0) { - return -errno; - } - - return 0; -} - -void RCUpdate::deinit() -{ - orb_unsubscribe(_rc_sub); - orb_unsubscribe(_rc_parameter_map_sub); } void RCUpdate::update_rc_functions() @@ -134,11 +102,8 @@ void RCUpdate::update_rc_functions() void RCUpdate::rc_parameter_map_poll(ParameterHandles ¶meter_handles, bool forced) { - bool map_updated; - orb_check(_rc_parameter_map_sub, &map_updated); - - if (map_updated) { - orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + if (_rc_parameter_map_sub.updated()) { + _rc_parameter_map_sub.copy(&_rc_parameter_map); /* update parameter handles to which the RC channels are mapped */ for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { @@ -253,14 +218,10 @@ RCUpdate::set_params_from_rc(const ParameterHandles ¶meter_handles) void RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) { - bool rc_updated; - orb_check(_rc_sub, &rc_updated); - - if (rc_updated) { + if (_rc_sub.updated()) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct input_rc_s rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + input_rc_s rc_input{}; + _rc_sub.copy(&rc_input); /* detect RC signal loss */ bool signal_lost; diff --git a/src/modules/sensors/rc_update.h b/src/modules/sensors/rc_update.h index a66cc80618..7e48a2a8f8 100644 --- a/src/modules/sensors/rc_update.h +++ b/src/modules/sensors/rc_update.h @@ -44,7 +44,9 @@ #include #include #include +#include #include +#include namespace sensors { @@ -63,17 +65,6 @@ public: */ RCUpdate(const Parameters ¶meters); - /** - * initialize subscriptions etc. - * @return 0 on success, <0 otherwise - */ - int init(); - - /** - * deinitialize the object (we cannot use the destructor because it is called on the wrong thread) - */ - void deinit(); - /** * Check for changes in rc_parameter_map */ @@ -111,17 +102,17 @@ private: void set_params_from_rc(const ParameterHandles ¶meter_handles); - int _rc_sub = -1; /**< raw rc channels data subscription */ - int _rc_parameter_map_sub = -1; /**< rc parameter map subscription */ + uORB::Subscription _rc_sub{ORB_ID(input_rc)}; /**< raw rc channels data subscription */ + uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; /**< rc parameter map subscription */ orb_advert_t _rc_pub = nullptr; /**< raw r/c control topic */ orb_advert_t _manual_control_pub = nullptr; /**< manual control signal topic */ orb_advert_t _actuator_group_3_pub = nullptr;/**< manual control as actuator topic */ - struct rc_channels_s _rc; /**< r/c channel data */ + rc_channels_s _rc {}; /**< r/c channel data */ - struct rc_parameter_map_s _rc_parameter_map; - float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ + rc_parameter_map_s _rc_parameter_map {}; + float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */ const Parameters &_parameters; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9eb07530fb..cb2ccf9137 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -163,10 +163,10 @@ private: const bool _hil_enabled; /**< if true, HIL is active */ bool _armed{false}; /**< arming status of the vehicle */ - int _actuator_ctrl_0_sub{-1}; /**< attitude controls sub */ - int _diff_pres_sub{-1}; /**< raw differential pressure subscription */ - int _vcontrol_mode_sub{-1}; /**< vehicle control mode subscription */ - int _params_sub{-1}; /**< notification of parameter updates */ + uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ + uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */ orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */ @@ -220,11 +220,6 @@ private: */ void diff_pres_poll(const vehicle_air_data_s &airdata); - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - /** * Check for changes in parameters. */ @@ -297,16 +292,9 @@ Sensors::adc_init() void Sensors::diff_pres_poll(const vehicle_air_data_s &raw) { - bool updated; - orb_check(_diff_pres_sub, &updated); + differential_pressure_s diff_pres{}; - if (updated) { - differential_pressure_s diff_pres; - int ret = orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &diff_pres); - - if (ret != PX4_OK) { - return; - } + if (_diff_pres_sub.update(&diff_pres)) { float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); @@ -360,33 +348,14 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) } } -void -Sensors::vehicle_control_mode_poll() -{ - struct vehicle_control_mode_s vcontrol_mode; - bool vcontrol_mode_updated; - - orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - - if (vcontrol_mode_updated) { - - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); - _armed = vcontrol_mode.flag_armed; - } -} - void Sensors::parameter_update_poll(bool forced) { - bool param_updated = false; - /* Check if any parameter has changed */ - orb_check(_params_sub, ¶m_updated); + parameter_update_s update; - if (param_updated || forced) { + if (_params_sub.update(&update) || forced) { /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); parameters_update(); updateParams(); @@ -557,10 +526,10 @@ Sensors::adc_poll() connected &= valid_chan[b]; } - actuator_controls_s ctrl; - orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); + actuator_controls_s ctrl{}; + _actuator_ctrl_0_sub.copy(&ctrl); - battery_status_s battery_status; + battery_status_s battery_status{}; _battery[b].updateBatteryStatus(t, bat_voltage_v[b], bat_current_a[b], connected, selected_source == b, b, ctrl.control[actuator_controls_s::INDEX_THROTTLE], @@ -592,20 +561,11 @@ Sensors::run() vehicle_air_data_s airdata = {}; vehicle_magnetometer_s magnetometer = {}; - _rc_update.init(); _voted_sensors_update.init(raw); /* (re)load params and calibration */ parameter_update_poll(true); - /* - * do subscriptions - */ - _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); - /* get a set of initial values */ _voted_sensors_update.sensors_poll(raw, airdata, magnetometer); @@ -649,7 +609,11 @@ Sensors::run() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_control_mode_poll(); + if (_vcontrol_mode_sub.updated()) { + vehicle_control_mode_s vcontrol_mode{}; + _vcontrol_mode_sub.copy(&vcontrol_mode); + _armed = vcontrol_mode.flag_armed; + } /* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro * a mandatory sensor) */ @@ -714,11 +678,6 @@ Sensors::run() perf_end(_loop_perf); } - orb_unsubscribe(_diff_pres_sub); - orb_unsubscribe(_vcontrol_mode_sub); - orb_unsubscribe(_params_sub); - orb_unsubscribe(_actuator_ctrl_0_sub); - if (_sensor_pub) { orb_unadvertise(_sensor_pub); } @@ -731,7 +690,6 @@ Sensors::run() orb_unadvertise(_magnetometer_pub); } - _rc_update.deinit(); _voted_sensors_update.deinit(); }