diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index 18f60a89cb..58f0ece0af 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -58,10 +58,6 @@ #include -#include -#include -#include - #include #include "crtp.h" @@ -100,9 +96,6 @@ Syslink::Syslink() : _fd(0), _queue(2, sizeof(syslink_message_t)), _writebuffer(16, sizeof(crtp_message_t)), - _battery_pub(nullptr), - _rc_pub(nullptr), - _cmd_pub(nullptr), _rssi(RC_INPUT_RSSI_MAX), _bstate(BAT_DISCHARGING) { @@ -434,14 +427,8 @@ Syslink::handle_message(syslink_message_t *msg) _bstate = BAT_DISCHARGING; } - // announce the battery status if needed, just publish else - if (_battery_pub != nullptr) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); - } + _battery_pub.publish(_battery_status); } else if (msg->type == SYSLINK_RADIO_RSSI) { uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm @@ -572,12 +559,7 @@ Syslink::handle_raw(syslink_message_t *sys) rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000; rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 - if (_rc_pub == nullptr) { - _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); - - } else { - orb_publish(ORB_ID(input_rc), _rc_pub, &rc); - } + _rc_pub.publish(rc); } else if (c->port == CRTP_PORT_MAVLINK) { _count_in++; diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index cc3665dead..79bdbce73f 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -40,7 +40,10 @@ #include #include -#include +#include +#include +#include +#include #include "syslink.h" #include "crtp.h" @@ -132,9 +135,8 @@ private: hrt_abstime _params_update[3]; // Time at which the parameters were updated hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module - orb_advert_t _battery_pub; - orb_advert_t _rc_pub; - orb_advert_t _cmd_pub; + uORB::PublicationMulti _battery_pub{ORB_ID(battery_status)}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; struct battery_status_s _battery_status;