From 20e6adc4ca52ef7f374c5514ec038b0b9a9b4223 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Sep 2019 20:44:03 -0400 Subject: [PATCH] sensors: update orb_publish to uORB::Publication<> --- src/modules/sensors/rc_update.cpp | 16 +++---- src/modules/sensors/rc_update.h | 11 +++-- src/modules/sensors/sensors.cpp | 46 ++++++-------------- src/modules/sensors/voted_sensors_update.cpp | 14 +----- src/modules/sensors/voted_sensors_update.h | 6 ++- 5 files changed, 33 insertions(+), 60 deletions(-) diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 9df56ac5af..bb8824a8eb 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -39,9 +39,6 @@ #include "rc_update.h" -#include -#include - using namespace sensors; RCUpdate::RCUpdate(const Parameters ¶meters) @@ -316,14 +313,13 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) _rc.frame_drop_count = rc_input.rc_lost_frame_count; /* publish rc_channels topic even if signal is invalid, for debug */ - int instance; - orb_publish_auto(ORB_ID(rc_channels), &_rc_pub, &_rc, &instance, ORB_PRIO_DEFAULT); + _rc_pub.publish(_rc); /* only publish manual control if the signal is still present and was present once */ if (!signal_lost && rc_input.timestamp_last_signal > 0) { /* initialize manual setpoint */ - struct manual_control_setpoint_s manual = {}; + manual_control_setpoint_s manual{}; /* set mode slot to unassigned */ manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE; /* set the timestamp to the last signal time */ @@ -405,11 +401,10 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) _parameters.rc_man_th, _parameters.rc_man_inv); /* publish manual_control_setpoint topic */ - orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_control_pub, &manual, &instance, - ORB_PRIO_HIGH); + _manual_control_pub.publish(manual); /* copy from mapped manual control to control group 3 */ - struct actuator_controls_s actuator_group_3 = {}; + actuator_controls_s actuator_group_3{}; actuator_group_3.timestamp = rc_input.timestamp_last_signal; @@ -423,8 +418,7 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ - orb_publish_auto(ORB_ID(actuator_controls_3), &_actuator_group_3_pub, &actuator_group_3, &instance, - ORB_PRIO_DEFAULT); + _actuator_group_3_pub.publish(actuator_group_3); /* Update parameters from RC Channels (tuning with RC) if activated */ if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) { diff --git a/src/modules/sensors/rc_update.h b/src/modules/sensors/rc_update.h index 7e48a2a8f8..5f805919d8 100644 --- a/src/modules/sensors/rc_update.h +++ b/src/modules/sensors/rc_update.h @@ -44,7 +44,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include @@ -105,9 +109,10 @@ private: 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 */ + uORB::Publication _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ + uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ + + uORB::PublicationMulti _manual_control_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ rc_channels_s _rc {}; /**< r/c channel data */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d28de3647e..ca6f16f7ad 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -168,12 +168,14 @@ private: 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 */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */ - orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */ - orb_advert_t _magnetometer_pub{nullptr}; /**< combined sensor data topic */ + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ + uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */ + uORB::Publication _sensor_preflight{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */ + uORB::Publication _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */ + uORB::Publication _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */ #if BOARD_NUMBER_BRICKS > 0 orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */ @@ -185,17 +187,13 @@ private: int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */ #endif /* BOARD_NUMBER_BRICKS > 1 */ - orb_advert_t _airspeed_pub{nullptr}; /**< airspeed */ - orb_advert_t _sensor_preflight{nullptr}; /**< sensor preflight topic */ - perf_counter_t _loop_perf; /**< loop performance counter */ DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL differential_pressure_s _diff_pres {}; - - orb_advert_t _diff_pres_pub{nullptr}; /**< differential_pressure */ + uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */ #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ Parameters _parameters{}; /**< local copies of interesting parameters */ @@ -358,8 +356,7 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) airspeed.air_temperature_celsius = air_temperature_celsius; if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) { - int instance; - orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &airspeed, &instance, ORB_PRIO_DEFAULT); + _airspeed_pub.publish(airspeed); } } } @@ -475,8 +472,7 @@ Sensors::adc_poll() (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; - int instance; - orb_publish_auto(ORB_ID(differential_pressure), &_diff_pres_pub, &_diff_pres, &instance, ORB_PRIO_DEFAULT); + _diff_pres_pub.publish(_diff_pres); } } else @@ -595,8 +591,6 @@ Sensors::run() _rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */); - _sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt); - /* wakeup source */ px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; @@ -653,15 +647,14 @@ Sensors::run() _voted_sensors_update.setRelativeTimestamps(raw); - int instance; - orb_publish_auto(ORB_ID(sensor_combined), &_sensor_pub, &raw, &instance, ORB_PRIO_DEFAULT); + _sensor_pub.publish(raw); if (airdata.timestamp != airdata_prev_timestamp) { - orb_publish_auto(ORB_ID(vehicle_air_data), &_airdata_pub, &airdata, &instance, ORB_PRIO_DEFAULT); + _airdata_pub.publish(airdata); } if (magnetometer.timestamp != magnetometer_prev_timestamp) { - orb_publish_auto(ORB_ID(vehicle_magnetometer), &_magnetometer_pub, &magnetometer, &instance, ORB_PRIO_DEFAULT); + _magnetometer_pub.publish(magnetometer); } _voted_sensors_update.checkFailover(); @@ -674,7 +667,8 @@ Sensors::run() _voted_sensors_update.calcAccelInconsistency(preflt); _voted_sensors_update.calcGyroInconsistency(preflt); _voted_sensors_update.calcMagInconsistency(preflt); - orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt); + + _sensor_preflight.publish(preflt); } } @@ -700,18 +694,6 @@ Sensors::run() perf_end(_loop_perf); } - if (_sensor_pub) { - orb_unadvertise(_sensor_pub); - } - - if (_airdata_pub) { - orb_unadvertise(_airdata_pub); - } - - if (_magnetometer_pub) { - orb_unadvertise(_magnetometer_pub); - } - _voted_sensors_update.deinit(); } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 13a51516cb..3d392e7a6f 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -1066,12 +1066,7 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s if (_corrections_changed) { _corrections.timestamp = hrt_absolute_time(); - if (_sensor_correction_pub == nullptr) { - _sensor_correction_pub = orb_advertise(ORB_ID(sensor_correction), &_corrections); - - } else { - orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections); - } + _sensor_correction_pub.publish(_corrections); _corrections_changed = false; } @@ -1080,12 +1075,7 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s if (_selection_changed) { _selection.timestamp = hrt_absolute_time(); - if (_sensor_selection_pub == nullptr) { - _sensor_selection_pub = orb_advertise(ORB_ID(sensor_selection), &_selection); - - } else { - orb_publish(ORB_ID(sensor_selection), _sensor_selection_pub, &_selection); - } + _sensor_selection_pub.publish(_selection); _selection_changed = false; } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 5894333f1f..cdb3548bc8 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -247,8 +248,9 @@ private: SensorData _baro {}; orb_advert_t _mavlink_log_pub{nullptr}; - orb_advert_t _sensor_correction_pub{nullptr}; /**< handle to the sensor correction uORB topic */ - orb_advert_t _sensor_selection_pub{nullptr}; /**< handle to the sensor selection uORB topic */ + + uORB::Publication _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */ + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */