diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index e102c71eb5..28963ebab9 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -331,14 +331,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad; _rates_sp.thrust_body[0] = _manual.z; - if (_rate_sp_pub != nullptr) { - /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else { - /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } + _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ @@ -748,14 +741,7 @@ void FixedwingAttitudeControl::Run() _rates_sp.timestamp = hrt_absolute_time(); - if (_rate_sp_pub != nullptr) { - /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else { - /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } + _rate_sp_pub.publish(_rates_sp); } else { vehicle_rates_setpoint_poll(); @@ -784,8 +770,7 @@ void FixedwingAttitudeControl::Run() rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); - int instance; - orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); + _rate_ctrl_status_pub.publish(rate_ctrl_status); } // Add feed-forward from roll control output to yaw control output @@ -817,14 +802,7 @@ void FixedwingAttitudeControl::Run() _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } - if (_actuators_2_pub != nullptr) { - /* publish the actuator controls*/ - orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); - - } else { - /* advertise and publish */ - _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); - } + _actuators_2_pub.publish(_actuators_airframe); } } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index b9c41d0856..651c206a67 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -47,6 +47,8 @@ #include #include #include +#include +#include #include #include #include @@ -108,14 +110,15 @@ private: uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; - orb_advert_t _rate_sp_pub{nullptr}; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */ - orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */ + uORB::Publication _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */ + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */ - orb_id_t _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure - orb_id_t _attitude_setpoint_id{nullptr}; + orb_id_t _attitude_setpoint_id{nullptr}; + orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */ + + orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */ actuator_controls_s _actuators {}; /**< actuator control inputs */ actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */