dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

crazyflie syslink update orb_publish to uORB::PublicationMulti<>

This commit is contained in:
Daniel Agar 2019-11-30 12:32:20 -05:00
parent e951531b12
commit 6a539a3836
2 changed files with 8 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
View File

@ -58,10 +58,6 @@
#include <systemlib/err.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/input_rc.h>
#include <board_config.h>
#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++;

10
boards/bitcraze/crazyflie/syslink/syslink_main.h
View File

@ -40,7 +40,10 @@
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/input_rc.h>
#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_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
struct battery_status_s _battery_status;