diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8270572e36..dc311506cf 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -76,6 +76,8 @@ #include #include +#include +#include #include #include #include @@ -262,11 +264,11 @@ private: bool _param_update_force; ///< force a parameter update /* advertised topics */ - orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_outputs; ///< mixed servo outputs topic - orb_advert_t _to_servorail; ///< servorail status - orb_advert_t _to_safety; ///< status of safety - orb_advert_t _to_mixer_status; ///< mixer status flags + uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; + uORB::PublicationMulti _to_outputs{ORB_ID(actuator_outputs)}; + uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; + uORB::Publication _to_servorail{ORB_ID(servorail_status)}; + uORB::Publication _to_safety{ORB_ID(safety)}; bool _primary_pwm_device; ///< true if we are the default PWM output bool _lockdown_override; ///< allow to override the safety lockdown @@ -474,11 +476,6 @@ PX4IO::PX4IO(device::Device *interface) : _last_written_arming_c(0), _t_actuator_controls_0(-1), _param_update_force(false), - _to_input_rc(nullptr), - _to_outputs(nullptr), - _to_servorail(nullptr), - _to_safety(nullptr), - _to_mixer_status(nullptr), _primary_pwm_device(false), _lockdown_override(false), _armed(false), @@ -1221,27 +1218,6 @@ out: unregister_driver(PWM_OUTPUT0_DEVICE_PATH); } - if (_to_input_rc) { - orb_unadvertise(_to_input_rc); - } - - if (_to_outputs) { - orb_unadvertise(_to_outputs); - } - - if (_to_servorail) { - orb_unadvertise(_to_servorail); - } - - if (_to_safety) { - orb_unadvertise(_to_safety); - } - - if (_to_mixer_status) { - orb_unadvertise(_to_mixer_status); - } - - /* tell the dtor that we are exiting */ _task = -1; _exit(0); @@ -1618,21 +1594,14 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct safety_s safety; + safety_s safety{}; safety.timestamp = hrt_absolute_time(); safety.safety_switch_available = true; safety.safety_off = (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? true : false; safety.override_available = _override_available; safety.override_enabled = (status & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? true : false; - /* lazily publish the safety status */ - if (_to_safety != nullptr) { - orb_publish(ORB_ID(safety), _to_safety, &safety); - - } else { - int instance; - _to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT); - } + _to_safety.publish(safety); return ret; } @@ -1671,7 +1640,7 @@ PX4IO::io_handle_alarms(uint16_t alarms) void PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) { - servorail_status_s servorail_status = {}; + servorail_status_s servorail_status{}; servorail_status.timestamp = hrt_absolute_time(); @@ -1690,12 +1659,7 @@ PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) } /* lazily publish the servorail voltages */ - if (_to_servorail != nullptr) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); - - } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); - } + _to_servorail.publish(servorail_status); } int @@ -1864,8 +1828,8 @@ PX4IO::io_publish_raw_rc() } } - int instance = 0; - orb_publish_auto(ORB_ID(input_rc), &_to_input_rc, &rc_val, &instance, ORB_PRIO_HIGH); + _to_input_rc.publish(rc_val); + return OK; } @@ -1889,8 +1853,7 @@ PX4IO::io_publish_pwm_outputs() outputs.output[i] = ctl[i]; } - int instance; - orb_publish_auto(ORB_ID(actuator_outputs), &_to_outputs, &outputs, &instance, ORB_PRIO_DEFAULT); + _to_outputs.publish(outputs); /* get mixer status flags from IO */ MultirotorMixer::saturation_status saturation_status; @@ -1902,11 +1865,11 @@ PX4IO::io_publish_pwm_outputs() /* publish mixer status */ if (saturation_status.flags.valid) { - multirotor_motor_limits_s motor_limits; + multirotor_motor_limits_s motor_limits{}; motor_limits.timestamp = hrt_absolute_time(); motor_limits.saturation_status = saturation_status.value; - orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT); + _to_mixer_status.publish(motor_limits); } return OK;