Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

crazyflie syslink update orb_publish to uORB::PublicationMulti<>

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
+2 -20
View File
@@ -58,10 +58,6 @@
#include <systemlib/err.h> #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 <board_config.h>
#include "crtp.h" #include "crtp.h"
@@ -100,9 +96,6 @@ Syslink::Syslink() :
_fd(0), _fd(0),
_queue(2, sizeof(syslink_message_t)), _queue(2, sizeof(syslink_message_t)),
_writebuffer(16, sizeof(crtp_message_t)), _writebuffer(16, sizeof(crtp_message_t)),
_battery_pub(nullptr),
_rc_pub(nullptr),
_cmd_pub(nullptr),
_rssi(RC_INPUT_RSSI_MAX), _rssi(RC_INPUT_RSSI_MAX),
_bstate(BAT_DISCHARGING) _bstate(BAT_DISCHARGING)
{ {
@@ -434,14 +427,8 @@ Syslink::handle_message(syslink_message_t *msg)
_bstate = BAT_DISCHARGING; _bstate = BAT_DISCHARGING;
} }
// announce the battery status if needed, just publish else // announce the battery status if needed, just publish else
if (_battery_pub != nullptr) { _battery_pub.publish(_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
} else if (msg->type == SYSLINK_RADIO_RSSI) { } else if (msg->type == SYSLINK_RADIO_RSSI) {
uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm 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[3] = cmd->thrust * 1000 / USHRT_MAX + 1000;
rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 rc.values[4] = 1000; // Dummy channel as px4 needs at least 5
if (_rc_pub == nullptr) { _rc_pub.publish(rc);
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
} else {
orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
}
} else if (c->port == CRTP_PORT_MAVLINK) { } else if (c->port == CRTP_PORT_MAVLINK) {
_count_in++; _count_in++;
boards/bitcraze/crazyflie/syslink/syslink_main.h
+6 -4
View File
@@ -40,7 +40,10 @@
#include <drivers/device/device.h> #include <drivers/device/device.h>
#include <drivers/device/ringbuffer.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 "syslink.h"
#include "crtp.h" #include "crtp.h"
@@ -132,9 +135,8 @@ private:
hrt_abstime _params_update[3]; // Time at which the parameters were updated 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 hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module
orb_advert_t _battery_pub; uORB::PublicationMulti<battery_status_s> _battery_pub{ORB_ID(battery_status)};
orb_advert_t _rc_pub; uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
orb_advert_t _cmd_pub;
struct battery_status_s _battery_status; struct battery_status_s _battery_status;