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

Syslink Bridge to Mavlink (#5479)

This commit is contained in:
Dennis Shtatnov 2016-09-11 05:23:15 -04:00 committed by Lorenz Meier
parent 446f8e3a36
commit 52fdbf4acc
10 changed files with 387 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rcS
View File

@ -525,6 +525,7 @@ then
if param compare SYS_COMPANION 20
then
syslink start
mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
fi
if param compare SYS_COMPANION 921600
then

2
src/drivers/boards/crazyflie/board_config.h
View File

@ -85,6 +85,8 @@ __BEGIN_DECLS
#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN13)
#define GPIO_NRF_TXEN (GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTA|GPIO_PIN4)
/*
* I2C busses

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -986,7 +986,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) {
/* no enough space in buffer to send */
/* not enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);

1
src/modules/syslink/CMakeLists.txt
View File

@ -36,6 +36,7 @@ px4_add_module(
STACK_MAIN 2000
SRCS
syslink_main.cpp
syslink_bridge.cpp
syslink_params.c
syslink.c
DEPENDS

5
src/modules/syslink/crtp.h
View File

@ -50,9 +50,12 @@
#define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3)
#define CRTP_MAX_DATA_SIZE 29
// 1 byte header + 31 bytes data = 32 (max ESB packet size)
// With the NRF51, this could be increased to ~250, but the Crazyradio PA uses a NRF24 which can't do this
#define CRTP_MAX_DATA_SIZE 31
typedef struct {
uint8_t size; // Total size of this message, including the header (placed here to overlap with syslink length field)
union {
uint8_t header;
struct {

1
src/modules/syslink/syslink.h
View File

@ -56,6 +56,7 @@ extern const char *syslink_magic;
#define SYSLINK_OW_READ 0x22
#define SYSLINK_OW_WRITE 0x23
// Limited by the CRTP packet which is limited by the ESB protocol used by the NRF
#define SYSLINK_MAX_DATA_LEN 32
#define SYSLINK_RADIO_RATE_250K 0

165
src/modules/syslink/syslink_bridge.cpp Normal file
View File

@ -0,0 +1,165 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file syslink_bridge.cpp
*
* Character device for talking to the radio as a plain serial port
*/
#include "syslink_main.h"
SyslinkBridge::SyslinkBridge(Syslink *link) :
CDev("SyslinkBridge", "/dev/bridge0"),
_link(link),
_readbuffer(16, sizeof(crtp_message_t))
{
}
SyslinkBridge::~SyslinkBridge()
{
}
int
SyslinkBridge::init()
{
int ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
return ret;
}
pollevent_t
SyslinkBridge::poll_state(struct file *filp)
{
pollevent_t state = 0;
if (!_readbuffer.empty()) {
state |= POLLIN;
}
if (_link->_writebuffer.space() > 0) {
state |= POLLOUT;
}
return 0;
}
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
ssize_t
SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen)
{
int nread = 0;
crtp_message_t msg;
while (!_readbuffer.empty() && buflen >= sizeof(CRTP_MAX_DATA_SIZE)) {
_readbuffer.get(&msg, sizeof(msg));
int size = msg.size - sizeof(msg.header);
memcpy(buffer, &msg.data, size);
nread += size;
buffer += size;
buflen -= size;
}
return nread;
}
ssize_t
SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen)
{
crtp_message_t msg;
// Queue and send next time we get a RAW radio packet
int remaining = buflen;
while (remaining > 0) {
int datasize = MIN(remaining, CRTP_MAX_DATA_SIZE);
msg.size = datasize + sizeof(msg.header);
msg.port = CRTP_PORT_MAVLINK;
memcpy(&msg.data, buffer, datasize);
_link->_writebuffer.force(&msg, sizeof(crtp_message_t));
buffer += datasize;
remaining -= datasize;
}
return buflen;
}
int
SyslinkBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
// All termios commands should be silently ignored as they are handled
switch (cmd) {
#ifdef FIONSPACE
case FIONSPACE:
#else
case FIONWRITE:
#endif
*((int *) arg) = _link->_writebuffer.space() * CRTP_MAX_DATA_SIZE;
return 0;
default:
/* give it to the superclass */
CDev::ioctl(filp, cmd, arg);
return 0;
}
}
void
SyslinkBridge::pipe_message(crtp_message_t *msg)
{
_readbuffer.force(msg, sizeof(msg->size) + msg->size);
poll_notify(POLLIN);
}

173
src/modules/syslink/syslink_main.cpp
View File

@ -70,6 +70,7 @@ Syslink::Syslink() :
_syslink_task(-1),
_task_running(false),
_fd(0),
_writebuffer(16, sizeof(crtp_message_t)),
_battery_pub(nullptr),
_rc_pub(nullptr),
_cmd_pub(nullptr),
@ -87,7 +88,7 @@ Syslink::start()
"syslink",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
3600,
1500,
Syslink::task_main_trampoline,
NULL
);
@ -127,6 +128,27 @@ Syslink::set_address(uint64_t addr)
}
int count_out = 0;
int
Syslink::send_queued_raw_message()
{
if (_writebuffer.empty()) {
return 0;
}
syslink_message_t msg;
msg.type = SYSLINK_RADIO_RAW;
count_out++;
_writebuffer.get(&msg.length, sizeof(crtp_message_t));
return send_message(&msg);
}
// 1M 8N1 serial connection to NRF51
int
Syslink::open_serial(const char *dev)
@ -154,7 +176,6 @@ Syslink::open_serial(const char *dev)
config.c_oflag &= ~ONLCR;
// Disable hardware flow control
// TODO: CF2 uses a TX EN as output flow control
config.c_cflag &= ~CRTSCTS;
@ -199,7 +220,8 @@ Syslink::task_main()
param_get(_param_radio_addr1, &addr + 4);
param_get(_param_radio_addr2, &addr);
_bridge = new SyslinkBridge(this);
_bridge->init();
_battery.reset(&_battery_status);
@ -217,9 +239,12 @@ Syslink::task_main()
/* Set non-blocking */
/*
int flags = fcntl(_fd, F_GETFL, 0);
fcntl(_fd, F_SETFL, flags | O_NONBLOCK);
*/
px4_arch_configgpio(GPIO_NRF_TXEN);
set_datarate(rate);
usleep(1000);
@ -283,6 +308,8 @@ Syslink::task_main()
static int count = 0;
static int null_count = 0;
static int count_in = 0;
//static int count_out = 0;
static hrt_abstime lasttime = 0;
@ -292,10 +319,12 @@ Syslink::handle_message(syslink_message_t *msg)
hrt_abstime t = hrt_absolute_time();
if (t - lasttime > 1000000) {
PX4_INFO("%d packets per second (%d null)", count, null_count);
PX4_INFO("%d p/s (%d null) (%d in / %d out raw)", count, null_count, count_in, count_out);
lasttime = t;
count = 0;
null_count = 0;
count_in = 0;
count_out = 0;
}
count++;
@ -313,7 +342,7 @@ Syslink::handle_message(syslink_message_t *msg)
float vbat;
memcpy(&vbat, &msg->data[1], sizeof(float));
_battery.updateBatteryStatus(t, vbat, 0, 0, false, &_battery_status);
_battery.updateBatteryStatus(t, vbat, -1, 0, false, &_battery_status);
// announce the battery status if needed, just publish else
if (_battery_pub != nullptr) {
@ -335,6 +364,9 @@ Syslink::handle_message(syslink_message_t *msg)
} else if (msg->type == SYSLINK_RADIO_RAW) {
handle_raw(msg);
} else {
PX4_INFO("GOT %d", msg->type);
}
@ -343,17 +375,14 @@ Syslink::handle_message(syslink_message_t *msg)
void
Syslink::handle_raw(syslink_message_t *sys)
{
// TODO: Now allow one raw message to be sent from the queue
crtp_message_t *c = (crtp_message_t *) &sys->data;
crtp_message_t *c = (crtp_message_t *) &sys->length;
if (CRTP_NULL(*c)) {
// TODO: Handle bootloader messages if possible
null_count++;
return;
}
if (c->port == CRTP_PORT_COMMANDER) {
} else if (c->port == CRTP_PORT_COMMANDER) {
crtp_commander *cmd = (crtp_commander *) &c->data[0];
@ -388,28 +417,136 @@ Syslink::handle_raw(syslink_message_t *sys)
}
} else if (c->port == CRTP_PORT_MAVLINK) {
count_in++;
/* Pipe to Mavlink bridge */
_bridge->pipe_message(c);
} else {
handle_raw_other(sys);
}
// Allow one raw message to be sent from the queue
send_queued_raw_message();
}
void
Syslink::handle_raw_other(syslink_message_t *sys)
{
// This function doesn't actually do anything
// It is just here to return null responses to most standard messages
crtp_message_t *c = (crtp_message_t *) &sys->length;
if (c->port == CRTP_PORT_LOG) {
PX4_INFO("Log: %d %d", c->channel, c->data[0]);
if (c->channel == 0) { // Table of Contents Access
uint8_t cmd = c->data[0];
if (cmd == 0) { // GET_ITEM
//int id = c->data[1];
memset(&c->data[2], 0, 3);
c->data[2] = 1; // type
c->size = 1 + 5;
send_message(sys);
} else if (cmd == 1) { // GET_INFO
memset(&c->data[1], 0, 7);
c->size = 1 + 8;
send_message(sys);
}
} else if (c->channel == 1) { // Log control
uint8_t cmd = c->data[0];
PX4_INFO("Responding to cmd: %d", cmd);
c->data[2] = 0; // Success
c->size = 3 + 1;
// resend message
send_message(sys);
} else if (c->channel == 2) { // Log data
}
} else if (c->port == CRTP_PORT_MEM) {
if (c->channel == 0) { // Info
int cmd = c->data[0];
if (cmd == 1) { // GET_NBR_OF_MEMS
c->data[1] = 0;
c->size = 2 + 1;
// resend message
send_message(sys);
}
}
} else if (c->port == CRTP_PORT_PARAM) {
if (c->channel == 0) { // TOC Access
// uint8_t msgId = c->data[0];
c->data[1] = 0; // Last parameter (id = 0)
memset(&c->data[2], 0, 10);
c->size = 1 + 8;
send_message(sys);
}
else if (c->channel == 1) { // Param read
// 0 is ok
c->data[1] = 0; // value
c->size = 1 + 3;
send_message(sys);
}
} else {
PX4_INFO("Got raw: %d", c->port);
}
}
int
Syslink::send_bytes(const void *data, size_t len)
{
// TODO: This could be way more efficient
// Using interrupts/DMA/polling would be much better
for (int i = 0; i < len; i++) {
// Block until we can send a byte
while (px4_arch_gpioread(GPIO_NRF_TXEN)) ;
write(_fd, ((const char *)data) + i, 1);
}
return 0;
}
int
Syslink::send_message(syslink_message_t *msg)
{
syslink_compute_cksum(msg);
write(_fd, syslink_magic, 2);
write(_fd, &msg->type, sizeof(msg->type));
write(_fd, &msg->length, sizeof(msg->length));
write(_fd, &msg->data, msg->length);
write(_fd, &msg->cksum, sizeof(msg->cksum));
send_bytes(syslink_magic, 2);
send_bytes(&msg->type, sizeof(msg->type));
send_bytes(&msg->length, sizeof(msg->length));
send_bytes(&msg->data, msg->length);
send_bytes(&msg->cksum, sizeof(msg->cksum));
return 0;
}
int syslink_main(int argc, char *argv[])
{
PX4_INFO("Starting daemon...");
g_syslink = new Syslink();
g_syslink->start();
// Wait for task and bridge to start
usleep(5000);
PX4_INFO("Started syslink on /dev/ttyS2");
return 0;
}

56
src/modules/syslink/syslink_main.h
View File

@ -32,9 +32,18 @@
****************************************************************************/
#include <stdint.h>
#include <systemlib/battery.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include "syslink.h"
#include "crtp.h"
class SyslinkBridge;
class Syslink
{
@ -50,19 +59,35 @@ public:
private:
friend class SyslinkBridge;
int open_serial(const char *dev);
void handle_message(syslink_message_t *msg);
void handle_raw(syslink_message_t *sys);
// Handles other types of messages that we don't really care about, but
// will be maintained with the bare minimum implementation for supporting
// other crazyflie libraries
void handle_raw_other(syslink_message_t *sys);
int send_bytes(const void *data, size_t len);
// Checksums and transmits a syslink message
int send_message(syslink_message_t *msg);
int send_queued_raw_message();
int _syslink_task;
bool _task_running;
int _fd;
// Stores data that was needs to be written as a raw message
ringbuffer::RingBuffer _writebuffer;
SyslinkBridge *_bridge;
orb_advert_t _battery_pub;
orb_advert_t _rc_pub;
orb_advert_t _cmd_pub;
@ -78,3 +103,34 @@ private:
void task_main();
};
class SyslinkBridge : public device::CDev
{
public:
SyslinkBridge(Syslink *link);
virtual ~SyslinkBridge();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
// Makes the message available for reading to processes reading from the bridge
void pipe_message(crtp_message_t *msg);
protected:
virtual pollevent_t poll_state(struct file *filp);
private:
Syslink *_link;
// Stores data that was received from syslink but not yet read by another driver
ringbuffer::RingBuffer _readbuffer;
};

2
src/modules/syslink/syslink_params.c
View File

@ -59,7 +59,7 @@ PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80);
* @max 2
* @group Syslink
*/
PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 0);
PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 2);
/**
* Operating address of the NRF51 (most significant byte)