diff --git a/src/modules/syslink/CMakeLists.txt b/src/modules/syslink/CMakeLists.txt index a46555159b..bb9ab5690a 100644 --- a/src/modules/syslink/CMakeLists.txt +++ b/src/modules/syslink/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS syslink_main.cpp syslink_bridge.cpp + syslink_memory.cpp syslink_params.c syslink.c DEPENDS diff --git a/src/modules/syslink/drv_deck.h b/src/modules/syslink/drv_deck.h new file mode 100644 index 0000000000..76a9500f25 --- /dev/null +++ b/src/modules/syslink/drv_deck.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /* Definitions for crazyflie related drivers */ + +#ifndef _DRV_CRAZYFLIE_H +#define _DRV_CRAZYFLIE_H + +#include +#include +#include + + +#define DECK_DEVICE_PATH "/dev/deck" + + + +/* structure of the data stored in deck memory */ +typedef struct { + uint8_t header; + uint32_t pins; + uint8_t vendorId; + uint8_t productId; + uint8_t crc; + uint8_t data[104]; + +} deck_descriptor_t; + + + +/* + * ioctl() definitions + */ + +#define _DECKIOCBASE (0x4100) +#define _DECKIOC(_n) (_PX4_IOC(_DECKIOCBASE, _n)) + +/** get the number of connected deck memory devices */ +#define DECKIOGNUM _DECKIOC(0) + +/** set the index of the current deck memory being accessed */ +#define DECKIOSNUM _DECKIOC(1) + +#define DECKIOID _DECKIOC(2) + + + + +#endif diff --git a/src/modules/syslink/syslink.h b/src/modules/syslink/syslink.h index 6c6ca16059..9f23060dda 100644 --- a/src/modules/syslink/syslink.h +++ b/src/modules/syslink/syslink.h @@ -37,7 +37,9 @@ extern const char *syslink_magic; +#define SYSLINK_GROUP 0xF0 +#define SYSLINK_RADIO 0x00 #define SYSLINK_RADIO_RAW 0x00 #define SYSLINK_RADIO_CHANNEL 0x01 #define SYSLINK_RADIO_DATARATE 0x02 @@ -45,12 +47,14 @@ extern const char *syslink_magic; #define SYSLINK_RADIO_RSSI 0x04 #define SYSLINK_RADIO_ADDRESS 0x05 +#define SYSLINK_PM 0x10 #define SYSLINK_PM_SOURCE 0x10 #define SYSLINK_PM_ONOFF_SWITCHOFF 0x11 #define SYSLINK_PM_BATTERY_VOLTAGE 0x12 #define SYSLINK_PM_BATTERY_STATE 0x13 #define SYSLINK_PM_BATTERY_AUTOUPDATE 0x14 +#define SYSLINK_OW 0x20 #define SYSLINK_OW_SCAN 0x20 #define SYSLINK_OW_GETINFO 0x21 #define SYSLINK_OW_READ 0x22 @@ -71,6 +75,34 @@ typedef struct { uint8_t cksum[2]; } syslink_message_t; + +#define OW_SIZE 112 +#define OW_READ_BLOCK 29 +#define OW_WRITE_BLOCK 27 + +typedef struct { + uint8_t nmems; +} syslink_ow_scan_t; + +typedef struct { + uint8_t idx; + uint8_t id; +} syslink_ow_getinfo_t; + +typedef struct { + uint8_t idx; + uint16_t addr; + uint8_t data[OW_READ_BLOCK]; +} syslink_ow_read_t; + +typedef struct { + uint8_t idx; + uint16_t addr; + uint16_t length; + uint8_t data[OW_WRITE_BLOCK]; +} syslink_ow_write_t; + + typedef enum { SYSLINK_STATE_START = 0, SYSLINK_STATE_TYPE, diff --git a/src/modules/syslink/syslink_bridge.cpp b/src/modules/syslink/syslink_bridge.cpp index edc90629e7..bc7a0600e0 100644 --- a/src/modules/syslink/syslink_bridge.cpp +++ b/src/modules/syslink/syslink_bridge.cpp @@ -88,8 +88,6 @@ SyslinkBridge::poll_state(struct file *filp) return 0; } -#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) - ssize_t SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen) { @@ -132,8 +130,6 @@ SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen) return buflen; } - - int SyslinkBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 58e0ddbacb..9c511d6911 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -87,7 +87,8 @@ Syslink::Syslink() : _rssi(RC_INPUT_RSSI_MAX), _bstate(BAT_DISCHARGING) { - + px4_sem_init(&radio_sem, 0, 0); + px4_sem_init(&memory_sem, 0, 0); } @@ -238,6 +239,9 @@ Syslink::task_main() _bridge = new SyslinkBridge(this); _bridge->init(); + _memory = new SyslinkMemory(this); + _memory->init(); + _battery.reset(&_battery_status); @@ -373,7 +377,7 @@ Syslink::handle_message(syslink_message_t *msg) _bstate = BAT_CHARGED; else _bstate = BAT_DISCHARGING; - + // announce the battery status if needed, just publish else if (_battery_pub != nullptr) { @@ -386,16 +390,16 @@ Syslink::handle_message(syslink_message_t *msg) } else if (msg->type == SYSLINK_RADIO_RSSI) { uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm _rssi = 140 - rssi * 100 / (100 - 40); - - } else if (msg->type == SYSLINK_RADIO_CHANNEL) { - PX4_INFO("Channel ACK %d", msg->data[0]); - - } else if (msg->type == SYSLINK_RADIO_DATARATE) { - PX4_INFO("Datarate ACK %d", msg->data[0]); - } else if (msg->type == SYSLINK_RADIO_RAW) { handle_raw(msg); lastrxtime = t; + + } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) { + radio_msg = *msg; + px4_sem_post(&radio_sem); + } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) { + _memory->msgbuf = *msg; + px4_sem_post(&memory_sem); } else { PX4_INFO("GOT %d", msg->type); } diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h index aa491d8e06..0b8d3b5838 100644 --- a/src/modules/syslink/syslink_main.h +++ b/src/modules/syslink/syslink_main.h @@ -43,6 +43,9 @@ #include "syslink.h" #include "crtp.h" +#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) + + typedef enum { BAT_DISCHARGING = 0, BAT_CHARGING = 1, @@ -51,6 +54,7 @@ typedef enum { class SyslinkBridge; +class SyslinkMemory; class Syslink { @@ -67,6 +71,7 @@ public: private: friend class SyslinkBridge; + friend class SyslinkMemory; int open_serial(const char *dev); @@ -94,6 +99,7 @@ private: // Stores data that was needs to be written as a raw message ringbuffer::RingBuffer _writebuffer; SyslinkBridge *_bridge; + SyslinkMemory *_memory; orb_advert_t _battery_pub; orb_advert_t _rc_pub; @@ -106,6 +112,12 @@ private: int32_t _rssi; battery_state _bstate; + px4_sem_t radio_sem; + px4_sem_t memory_sem; + + syslink_message_t radio_msg; + syslink_message_t memory_msg; + static int task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -142,3 +154,36 @@ private: }; + + +class SyslinkMemory : public device::CDev +{ + +public: + SyslinkMemory(Syslink *link); + virtual ~SyslinkMemory(); + + 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); + +private: + friend class Syslink; + + Syslink *_link; + + int _activeI; + + syslink_message_t msgbuf; + + uint8_t scan(); + void getinfo(int i); + + int read(int i, uint16_t addr, char *buf, int length); + int write(int i, uint16_t addr, const char *buf, int length); + + void sendAndWait(); + +}; diff --git a/src/modules/syslink/syslink_memory.cpp b/src/modules/syslink/syslink_memory.cpp new file mode 100644 index 0000000000..4d6f6155d0 --- /dev/null +++ b/src/modules/syslink/syslink_memory.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +#include "syslink_main.h" + +#include "drv_deck.h" + + +SyslinkMemory::SyslinkMemory(Syslink *link) : + CDev("SyslinkMemory", DECK_DEVICE_PATH), + _link(link), + _activeI(0) +{ + + +} + +SyslinkMemory::~SyslinkMemory() +{ + +} + + +int +SyslinkMemory::init() +{ + int ret = CDev::init(); + + /* if init failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + return ret; + } + + + return ret; +} + +ssize_t +SyslinkMemory::read(struct file *filp, char *buffer, size_t buflen) +{ + return read(_activeI, 0, buffer, buflen); +} + +ssize_t +SyslinkMemory::write(struct file *filp, const char *buffer, size_t buflen) +{ + // For now, unsupported + return -1; +// return buflen; +} + +int +SyslinkMemory::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case DECKIOGNUM: + *((int *) arg) = scan(); + case DECKIOSNUM: + _activeI = *((int *) arg); + return 0; + + case DECKIOID: + syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data; + getinfo(_activeI) + arg = &data->index; + return sizeof(data->index); + + default: + CDev::ioctl(filp, cmd, arg); + return 0; + } +} + + +uint8_t +SyslinkMemory::scan() +{ + syslink_ow_scan_t *data = (syslink_ow_scan_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_SCAN; + msgbuf.length = 0; + sendAndWait(); + + return data->nmems; +} + +void +SyslinkMemory::getinfo(int i) +{ + syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_SCAN; + msgbuf.length = 1; + data->idx = i; + sendAndWait(); +} + +int +SyslinkMemory::read(int i, uint16_t addr, char *buf, int length) +{ + syslink_ow_read_t *data = (syslink_ow_read_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_READ; + + int nread = 0; + while (nread < length) { + + msgbuf.length = 3; + data->idx = i; + data->addr = addr; + sendAndWait(); + + // Number of bytes actually read + int n = MIN(length - nread, msgbuf.length - 3); + if(n == 0) + break; + + memcpy(buf, data->data, n); + nread += n; + buf += n; + } + + return nread; +} + +int +SyslinkMemory::write(int i, uint16_t addr, const char *buf, int length) +{ + // TODO: Unimplemented + return -1; +} + +void +SyslinkMemory::sendAndWait() +{ + // TODO: Mutex lock sending a message + _link->send_message(&msgbuf); + px4_sem_wait(&_link->memory_sem); +}