diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7cc62ab95f..012459ba67 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index 04b7c693b7..84f32b0cd3 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 63c9301382..8cd0111242 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); diff --git a/src/modules/syslink/CMakeLists.txt b/src/modules/syslink/CMakeLists.txt index e2f02fc53d..a46555159b 100644 --- a/src/modules/syslink/CMakeLists.txt +++ b/src/modules/syslink/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_module( STACK_MAIN 2000 SRCS syslink_main.cpp + syslink_bridge.cpp syslink_params.c syslink.c DEPENDS diff --git a/src/modules/syslink/crtp.h b/src/modules/syslink/crtp.h index 596dc3429c..a874659a25 100644 --- a/src/modules/syslink/crtp.h +++ b/src/modules/syslink/crtp.h @@ -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 { diff --git a/src/modules/syslink/syslink.h b/src/modules/syslink/syslink.h index 2038bad7d9..6c6ca16059 100644 --- a/src/modules/syslink/syslink.h +++ b/src/modules/syslink/syslink.h @@ -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 diff --git a/src/modules/syslink/syslink_bridge.cpp b/src/modules/syslink/syslink_bridge.cpp new file mode 100644 index 0000000000..edc90629e7 --- /dev/null +++ b/src/modules/syslink/syslink_bridge.cpp @@ -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); +} diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 91d7aec861..67336f01a2 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -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; } diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h index 3b216fcf24..b375253a49 100644 --- a/src/modules/syslink/syslink_main.h +++ b/src/modules/syslink/syslink_main.h @@ -32,9 +32,18 @@ ****************************************************************************/ #include + #include + +#include +#include + #include + #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; + + +}; diff --git a/src/modules/syslink/syslink_params.c b/src/modules/syslink/syslink_params.c index 0d6ac9d087..08844cba45 100644 --- a/src/modules/syslink/syslink_params.c +++ b/src/modules/syslink/syslink_params.c @@ -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)