From dd9b0ded0fd4cfeba7c6d44d5023a96529f7d5f9 Mon Sep 17 00:00:00 2001 From: TheLegendaryJedi Date: Tue, 5 Jan 2021 22:15:00 +0000 Subject: [PATCH] [UPDATE] - Remove duplicate syslink code --- boards/bitcraze/crazyflie21/CMakeLists.txt | 2 +- .../crazyflie21/syslink/CMakeLists.txt | 47 - boards/bitcraze/crazyflie21/syslink/crtp.h | 77 -- .../bitcraze/crazyflie21/syslink/drv_deck.h | 79 -- .../crazyflie21/syslink/ringbuffer.cpp | 418 --------- .../bitcraze/crazyflie21/syslink/ringbuffer.h | 179 ---- boards/bitcraze/crazyflie21/syslink/syslink.c | 153 ---- boards/bitcraze/crazyflie21/syslink/syslink.h | 141 --- .../crazyflie21/syslink/syslink_bridge.cpp | 162 ---- .../crazyflie21/syslink/syslink_main.cpp | 865 ------------------ .../crazyflie21/syslink/syslink_main.h | 216 ----- .../crazyflie21/syslink/syslink_memory.cpp | 170 ---- .../crazyflie21/syslink/syslink_params.c | 72 -- 13 files changed, 1 insertion(+), 2580 deletions(-) delete mode 100644 boards/bitcraze/crazyflie21/syslink/CMakeLists.txt delete mode 100644 boards/bitcraze/crazyflie21/syslink/crtp.h delete mode 100644 boards/bitcraze/crazyflie21/syslink/drv_deck.h delete mode 100644 boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp delete mode 100644 boards/bitcraze/crazyflie21/syslink/ringbuffer.h delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink.c delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink.h delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink_main.cpp delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink_main.h delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp delete mode 100644 boards/bitcraze/crazyflie21/syslink/syslink_params.c diff --git a/boards/bitcraze/crazyflie21/CMakeLists.txt b/boards/bitcraze/crazyflie21/CMakeLists.txt index 4fda6f229e..279415618d 100644 --- a/boards/bitcraze/crazyflie21/CMakeLists.txt +++ b/boards/bitcraze/crazyflie21/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -add_subdirectory(syslink) +add_subdirectory(../crazyflie syslink) diff --git a/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt b/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt deleted file mode 100644 index ebf6c04c08..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -px4_add_module( - MODULE modules__syslink - MAIN syslink - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable - SRCS - ringbuffer.cpp - syslink_main.cpp - syslink_bridge.cpp - syslink_memory.cpp - syslink.c - DEPENDS - battery - ) diff --git a/boards/bitcraze/crazyflie21/syslink/crtp.h b/boards/bitcraze/crazyflie21/syslink/crtp.h deleted file mode 100644 index a874659a25..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/crtp.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - - -#include - -#define CRTP_PORT_CONSOLE 0x00 -#define CRTP_PORT_PARAM 0x02 -#define CRTP_PORT_COMMANDER 0x03 -#define CRTP_PORT_MEM 0x04 -#define CRTP_PORT_LOG 0x05 - -#define CRTP_PORT_MAVLINK 0x08 // Non-standard port for transmitting mavlink messages - -#define CRTP_PORT_PLATFORM 0x0D -#define CRTP_PORT_DEBUG 0x0E -#define CRTP_PORT_LINK 0x0F - -#define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3) - -// 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 { - uint8_t channel : 2; - uint8_t link : 2; - uint8_t port : 4; - }; - }; - - uint8_t data[CRTP_MAX_DATA_SIZE]; -} __attribute__((packed)) crtp_message_t; - - -typedef struct { - float roll; // -20 to 20 - float pitch; - float yaw; // -150 to 150 - uint16_t thrust; -} crtp_commander; diff --git a/boards/bitcraze/crazyflie21/syslink/drv_deck.h b/boards/bitcraze/crazyflie21/syslink/drv_deck.h deleted file mode 100644 index 2b21ae5f3f..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/drv_deck.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * 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; // Should be 0xEB - uint32_t pins; - uint8_t vendorId; - uint8_t productId; - uint8_t crc; - uint8_t data[104]; - -} __attribute__((packed)) 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/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp b/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp deleted file mode 100644 index 92c9fcf753..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp +++ /dev/null @@ -1,418 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 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 ringbuffer.cpp - * - * A flexible ringbuffer class. - */ - -#include "ringbuffer.h" -#include - -namespace ringbuffer -{ - -RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : - _num_items(num_items), - _item_size(item_size), - _buf(new char[(_num_items + 1) * item_size]), - _head(_num_items), - _tail(_num_items) -{} - -RingBuffer::~RingBuffer() -{ - delete[] _buf; -} - -unsigned -RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _num_items : (index - 1); -} - -bool -RingBuffer::empty() -{ - return _tail == _head; -} - -bool -RingBuffer::full() -{ - return _next(_head) == _tail; -} - -unsigned -RingBuffer::size() -{ - return (_buf != nullptr) ? _num_items : 0; -} - -void -RingBuffer::flush() -{ - while (!empty()) { - get(nullptr); - } -} - -bool -RingBuffer::put(const void *val, size_t val_size) -{ - unsigned next = _next(_head); - - if (next != _tail) { - if ((val_size == 0) || (val_size > _item_size)) { - val_size = _item_size; - } - - memcpy(&_buf[_head * _item_size], val, val_size); - _head = next; - return true; - - } else { - return false; - } -} - -bool -RingBuffer::put(int8_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint8_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int16_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint16_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int32_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint32_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int64_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint64_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(float val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(double val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::force(const void *val, size_t val_size) -{ - bool overwrote = false; - - for (;;) { - if (put(val, val_size)) { - break; - } - - get(nullptr); - overwrote = true; - } - - return overwrote; -} - -bool -RingBuffer::force(int8_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint8_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int16_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint16_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int32_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint32_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int64_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint64_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(float val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(double val) -{ - return force(&val, sizeof(val)); -} - -// FIXME - clang crashes on this get() call -#ifdef __PX4_QURT -#define __PX4_SBCAP my_sync_bool_compare_and_swap -static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) -{ - if (*a == b) { - *a = c; - return true; - } - - return false; -} - -#else -#define __PX4_SBCAP __sync_bool_compare_and_swap -#endif -bool -RingBuffer::get(void *val, size_t val_size) -{ - if (_tail != _head) { - unsigned candidate; - unsigned next; - - if ((val_size == 0) || (val_size > _item_size)) { - val_size = _item_size; - } - - do { - /* decide which element we think we're going to read */ - candidate = _tail; - - /* and what the corresponding next index will be */ - next = _next(candidate); - - /* go ahead and read from this index */ - if (val != nullptr) { - memcpy(val, &_buf[candidate * _item_size], val_size); - } - - /* if the tail pointer didn't change, we got our item */ - } while (!__PX4_SBCAP(&_tail, candidate, next)); - - return true; - - } else { - return false; - } -} - -bool -RingBuffer::get(int8_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint8_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int16_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint16_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int32_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint32_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int64_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint64_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(float &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(double &val) -{ - return get(&val, sizeof(val)); -} - -unsigned -RingBuffer::space() -{ - unsigned tail, head; - - /* - * Make a copy of the head/tail pointers in a fashion that - * may err on the side of under-estimating the free space - * in the buffer in the case that the buffer is being updated - * asynchronously with our check. - * If the head pointer changes (reducing space) while copying, - * re-try the copy. - */ - do { - head = _head; - tail = _tail; - } while (head != _head); - - return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); -} - -unsigned -RingBuffer::count() -{ - /* - * Note that due to the conservative nature of space(), this may - * over-estimate the number of items in the buffer. - */ - return _num_items - space(); -} - -bool -RingBuffer::resize(unsigned new_size) -{ - char *old_buffer; - char *new_buffer = new char [(new_size + 1) * _item_size]; - - if (new_buffer == nullptr) { - return false; - } - - old_buffer = _buf; - _buf = new_buffer; - _num_items = new_size; - _head = new_size; - _tail = new_size; - delete[] old_buffer; - return true; -} - -void -RingBuffer::print_info(const char *name) -{ - printf("%s %u/%lu (%u/%u @ %p)\n", - name, - _num_items, - (unsigned long)_num_items * _item_size, - _head, - _tail, - _buf); -} - -} // namespace ringbuffer diff --git a/boards/bitcraze/crazyflie21/syslink/ringbuffer.h b/boards/bitcraze/crazyflie21/syslink/ringbuffer.h deleted file mode 100644 index 04bd9fa973..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/ringbuffer.h +++ /dev/null @@ -1,179 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 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 ringbuffer.h - * - * A flexible ringbuffer class. - */ - -#pragma once - -#include -#include -#include - -namespace ringbuffer __EXPORT -{ - -class RingBuffer -{ -public: - RingBuffer(unsigned num_items, size_t item_size); - virtual ~RingBuffer(); - - /** - * Put an item into the buffer. - * - * @param val Item to put - * @return true if the item was put, false if the buffer is full - */ - bool put(const void *val, size_t val_size = 0); - - bool put(int8_t val); - bool put(uint8_t val); - bool put(int16_t val); - bool put(uint16_t val); - bool put(int32_t val); - bool put(uint32_t val); - bool put(int64_t val); - bool put(uint64_t val); - bool put(float val); - bool put(double val); - - /** - * Force an item into the buffer, discarding an older item if there is not space. - * - * @param val Item to put - * @return true if an item was discarded to make space - */ - bool force(const void *val, size_t val_size = 0); - - bool force(int8_t val); - bool force(uint8_t val); - bool force(int16_t val); - bool force(uint16_t val); - bool force(int32_t val); - bool force(uint32_t val); - bool force(int64_t val); - bool force(uint64_t val); - bool force(float val); - bool force(double val); - - /** - * Get an item from the buffer. - * - * @param val Item that was gotten - * @return true if an item was got, false if the buffer was empty. - */ - bool get(void *val, size_t val_size = 0); - - bool get(int8_t &val); - bool get(uint8_t &val); - bool get(int16_t &val); - bool get(uint16_t &val); - bool get(int32_t &val); - bool get(uint32_t &val); - bool get(int64_t &val); - bool get(uint64_t &val); - bool get(float &val); - bool get(double &val); - - /* - * Get the number of slots free in the buffer. - * - * @return The number of items that can be put into the buffer before - * it becomes full. - */ - unsigned space(void); - - /* - * Get the number of items in the buffer. - * - * @return The number of items that can be got from the buffer before - * it becomes empty. - */ - unsigned count(void); - - /* - * Returns true if the buffer is empty. - */ - bool empty(); - - /* - * Returns true if the buffer is full. - */ - bool full(); - - /* - * Returns the capacity of the buffer, or zero if the buffer could - * not be allocated. - */ - unsigned size(); - - /* - * Empties the buffer. - */ - void flush(); - - /* - * resize the buffer. This is unsafe to be called while - * a producer or consuming is running. Caller is responsible - * for any locking needed - * - * @param new_size new size for buffer - * @return true if the resize succeeds, false if - * not (allocation error) - */ - bool resize(unsigned new_size); - - /* - * printf() some info on the buffer - */ - void print_info(const char *name); - -private: - unsigned _num_items; - const size_t _item_size; - char *_buf; - volatile unsigned _head; /**< insertion point in _item_size units */ - volatile unsigned _tail; /**< removal point in _item_size units */ - - unsigned _next(unsigned index); - - /* we don't want this class to be copied */ - RingBuffer(const RingBuffer &); - RingBuffer operator=(const RingBuffer &); -}; - -} // namespace ringbuffer diff --git a/boards/bitcraze/crazyflie21/syslink/syslink.c b/boards/bitcraze/crazyflie21/syslink/syslink.c deleted file mode 100644 index d9e40d137f..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink.c +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * 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.c - * - * Crazyflie Syslink protocol implementation - * - * @author Dennis Shtatnov - */ - - -#include -#include -#include -#include -#include -#include -#include - - -#include "syslink.h" - - -const char *syslink_magic = "\xbc\xcf"; - -void syslink_parse_init(syslink_parse_state *state) -{ - state->state = SYSLINK_STATE_START; - state->index = 0; -} - - -int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg) -{ - - switch (state->state) { - case SYSLINK_STATE_START: - if (c == syslink_magic[state->index]) { - state->index++; - - } else { - state->index = 0; - } - - if (syslink_magic[state->index] == '\x00') { - state->state = SYSLINK_STATE_TYPE; - } - - break; - - case SYSLINK_STATE_TYPE: - msg->type = c; - state->state = SYSLINK_STATE_LENGTH; - break; - - case SYSLINK_STATE_LENGTH: - msg->length = c; - - if (c > SYSLINK_MAX_DATA_LEN) { // Too long - state->state = SYSLINK_STATE_START; - - } else { - state->state = c > 0 ? SYSLINK_STATE_DATA : SYSLINK_STATE_CKSUM; - } - - state->index = 0; - break; - - case SYSLINK_STATE_DATA: - msg->data[state->index++] = c; - - if (state->index >= msg->length) { - state->state = SYSLINK_STATE_CKSUM; - state->index = 0; - syslink_compute_cksum(msg); - } - - break; - - case SYSLINK_STATE_CKSUM: - if (c != msg->cksum[state->index]) { - PX4_INFO("Bad checksum"); - state->state = SYSLINK_STATE_START; - state->index = 0; - break; - } - - state->index++; - - if (state->index >= (int)sizeof(msg->cksum)) { - state->state = SYSLINK_STATE_START; - state->index = 0; - return 1; - } - - - break; - } - - return 0; - -} - -/* - Computes Fletcher 8bit checksum per RFC1146 - A := A + D[i] - B := B + A -*/ -void syslink_compute_cksum(syslink_message_t *msg) -{ - uint8_t a = 0, b = 0; - uint8_t *Di = (uint8_t *)msg, *end = Di + (2 + msg->length) * sizeof(uint8_t); - - while (Di < end) { - a = a + *Di; - b = b + a; - ++Di; - } - - msg->cksum[0] = a; - msg->cksum[1] = b; -} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink.h b/boards/bitcraze/crazyflie21/syslink/syslink.h deleted file mode 100644 index d0ccb14c4b..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink.h +++ /dev/null @@ -1,141 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#include - - -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 -#define SYSLINK_RADIO_CONTWAVE 0x03 -#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 -#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 -#define SYSLINK_RADIO_RATE_1M 1 -#define SYSLINK_RADIO_RATE_2M 2 - - -typedef struct { - uint8_t type; - uint8_t length; - uint8_t data[SYSLINK_MAX_DATA_LEN]; - uint8_t cksum[2]; -} __attribute__((packed)) syslink_message_t; - - -#define OW_SIZE 112 -#define OW_READ_BLOCK 29 -#define OW_WRITE_BLOCK 26 // TODO: Use even, but can be up to 27 - -typedef struct { - uint8_t nmems; -} __attribute__((packed)) syslink_ow_scan_t; - -typedef struct { - uint8_t family; // Should by 0x0D for most chips - uint8_t sn[6]; - uint8_t crc; -} __attribute__((packed)) syslink_ow_id_t; - -typedef struct { - uint8_t idx; - uint8_t id[8]; -} __attribute__((packed)) syslink_ow_getinfo_t; - -typedef struct { - uint8_t idx; - uint16_t addr; - uint8_t data[OW_READ_BLOCK]; -} __attribute__((packed)) syslink_ow_read_t; - -typedef struct { - uint8_t idx; - uint16_t addr; - uint16_t length; - uint8_t data[OW_WRITE_BLOCK]; -} __attribute__((packed)) syslink_ow_write_t; - - -typedef enum { - SYSLINK_STATE_START = 0, - SYSLINK_STATE_TYPE, - SYSLINK_STATE_LENGTH, - SYSLINK_STATE_DATA, - SYSLINK_STATE_CKSUM -} SYSLINK_STATE; - -typedef struct { - SYSLINK_STATE state; - int index; - -} syslink_parse_state; - - -#ifdef __cplusplus -extern "C" { -#endif - -extern void syslink_parse_init(syslink_parse_state *state); - -// Returns true if a full message was parsed -extern int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg); - -extern void syslink_compute_cksum(syslink_message_t *msg); - -#ifdef __cplusplus -} -#endif diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp deleted file mode 100644 index 7c7ac1e21a..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/**************************************************************************** - * - * 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" -#include - - - -SyslinkBridge::SyslinkBridge(Syslink *link) : - CDev("/dev/bridge0"), - _link(link), - _readbuffer(16, sizeof(crtp_message_t)) -{ - _msg_to_send.header = 0; - _msg_to_send.size = sizeof(_msg_to_send.header); - _msg_to_send.port = CRTP_PORT_MAVLINK; - _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; // to send 30 bytes of data - //ideally _msg_to_send.data size should be CRTP_MAX_DATA_SIZE but cfbridge does not receive 31 bytes of data due to a bug somewhere -} - -int -SyslinkBridge::init() -{ - int ret = CDev::init(); - - /* if init failed, bail now */ - if (ret != OK) { - PX4_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 state; -} - -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) -{ - int buflen_rem = buflen; - - while (buflen_rem > 0) { - - int datasize = MIN(_msg_to_send_size_remaining, buflen_rem); - _msg_to_send.size += datasize; - memcpy(&_msg_to_send.data[CRTP_MAX_DATA_SIZE - 1 - _msg_to_send_size_remaining], buffer, datasize); - - buffer += datasize; - _msg_to_send_size_remaining -= datasize; - buflen_rem -= datasize; - - - if (_msg_to_send_size_remaining == 0) { - - if (_link->_writebuffer.force(&_msg_to_send, sizeof(crtp_message_t))) { - PX4_WARN("write buffer overflow"); - } - - _msg_to_send.size = sizeof(_msg_to_send.header); - _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; - } - } - - 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) { - - case FIONSPACE: - *((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/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp deleted file mode 100644 index b6e7b06aaf..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp +++ /dev/null @@ -1,865 +0,0 @@ -/**************************************************************************** - * - * 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_main.cpp - * Entry point for syslink module used to communicate with the NRF module on a Crazyflie - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include "crtp.h" -#include "syslink_main.h" -#include "drv_deck.h" - - - -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); -__END_DECLS - -extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); } - - -Syslink *g_syslink = nullptr; - -Syslink::Syslink() : - pktrate(0), - nullrate(0), - rxrate(0), - txrate(0), - _syslink_task(-1), - _task_running(false), - _bootloader_mode(false), - _count(0), - _null_count(0), - _count_in(0), - _count_out(0), - _lasttime(0), - _lasttxtime(0), - _lastrxtime(0), - _fd(0), - _queue(2, sizeof(syslink_message_t)), - _writebuffer(16, sizeof(crtp_message_t)), - _rssi(RC_INPUT_RSSI_MAX), - _bstate(BAT_DISCHARGING) -{ - px4_sem_init(&memory_sem, 0, 0); - /* memory_sem use case is a signal */ - px4_sem_setprotocol(&memory_sem, SEM_PRIO_NONE); -} - - -int -Syslink::start() -{ - _task_running = true; - _syslink_task = px4_task_spawn_cmd( - "syslink", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1500, - Syslink::task_main_trampoline, - NULL - ); - - return 0; -} - - -int -Syslink::set_datarate(uint8_t datarate) -{ - syslink_message_t msg; - msg.type = SYSLINK_RADIO_DATARATE; - msg.length = 1; - msg.data[0] = datarate; - return send_message(&msg); -} - -int -Syslink::set_channel(uint8_t channel) -{ - syslink_message_t msg; - msg.type = SYSLINK_RADIO_CHANNEL; - msg.length = 1; - msg.data[0] = channel; - return send_message(&msg); -} - -int -Syslink::set_address(uint64_t addr) -{ - syslink_message_t msg; - msg.type = SYSLINK_RADIO_ADDRESS; - msg.length = 5; - memcpy(&msg.data, &addr, 5); - return send_message(&msg); -} - -int -Syslink::send_queued_raw_message() -{ - if (_writebuffer.empty()) { - return 0; - } - - _lasttxtime = hrt_absolute_time(); - - syslink_message_t msg; - msg.type = SYSLINK_RADIO_RAW; - - _count_out++; - - _writebuffer.get(&msg.length, sizeof(crtp_message_t)); - - return send_message(&msg); -} - - -void -Syslink::update_params(bool force_set) -{ - param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN"); - param_t _param_radio_rate = param_find("SLNK_RADIO_RATE"); - param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1"); - param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2"); - - - // reading parameter values into temp variables - - int32_t channel, rate, addr1, addr2; - uint64_t addr = 0; - - param_get(_param_radio_channel, &channel); - param_get(_param_radio_rate, &rate); - param_get(_param_radio_addr1, &addr1); - param_get(_param_radio_addr2, &addr2); - - memcpy(&addr, &addr2, 4); - memcpy(((char *)&addr) + 4, &addr1, 4); - - - hrt_abstime t = hrt_absolute_time(); - - // request updates if needed - - if (channel != this->_channel || force_set) { - this->_channel = channel; - set_channel(channel); - this->_params_update[0] = t; - this->_params_ack[0] = 0; - } - - if (rate != this->_rate || force_set) { - this->_rate = rate; - set_datarate(rate); - this->_params_update[1] = t; - this->_params_ack[1] = 0; - } - - if (addr != this->_addr || force_set) { - this->_addr = addr; - set_address(addr); - this->_params_update[2] = t; - this->_params_ack[2] = 0; - } -} - -// 1M 8N1 serial connection to NRF51 -int -Syslink::open_serial(const char *dev) -{ -#ifndef B1000000 -#define B1000000 1000000 -#endif - - int rate = B1000000; - - // open uart - int fd = px4_open(dev, O_RDWR | O_NOCTTY); - int termios_state = -1; - - if (fd < 0) { - PX4_ERR("failed to open uart device!"); - return -1; - } - - // set baud rate - struct termios config; - tcgetattr(fd, &config); - - // clear ONLCR flag (which appends a CR for every LF) - config.c_oflag = 0; - config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - // Disable hardware flow control - config.c_cflag &= ~CRTSCTS; - - - /* Set baud rate */ - if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) { - warnx("ERR SET BAUD %s: %d\n", dev, termios_state); - px4_close(fd); - return -1; - } - - if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) { - PX4_WARN("ERR SET CONF %s\n", dev); - px4_close(fd); - return -1; - } - - return fd; -} - - - -int -Syslink::task_main_trampoline(int argc, char *argv[]) -{ - g_syslink->task_main(); - return 0; -} - -void -Syslink::task_main() -{ - _bridge = new SyslinkBridge(this); - _bridge->init(); - - _memory = new SyslinkMemory(this); - _memory->init(); - - _battery.reset(); - - - // int ret; - - /* Open serial port */ - const char *dev = "/dev/ttyS2"; - _fd = open_serial(dev); - - if (_fd < 0) { - err(1, "can't open %s", dev); - return; - } - - - /* Set non-blocking */ - /* - int flags = fcntl(_fd, F_GETFL, 0); - fcntl(_fd, F_SETFL, flags | O_NONBLOCK); - */ - - px4_arch_configgpio(GPIO_NRF_TXEN); - - px4_pollfd_struct_t fds[2]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - fds[1].fd = _params_sub; - fds[1].events = POLLIN; - - int error_counter = 0; - - char buf[64]; - int nread; - - syslink_parse_state state; - syslink_message_t msg; - - syslink_parse_init(&state); - - //setup initial parameters - update_params(true); - - while (_task_running) { - int poll_ret = px4_poll(fds, 2, 500); - - /* handle the poll result */ - if (poll_ret == 0) { - /* timeout: this means none of our providers is giving us data */ - - } else if (poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - if (error_counter < 10 || error_counter % 50 == 0) { - /* use a counter to prevent flooding (and slowing us down) */ - PX4_ERR("[syslink] ERROR return value from poll(): %d" - , poll_ret); - } - - error_counter++; - - } else { - if (fds[0].revents & POLLIN) { - if ((nread = read(_fd, buf, sizeof(buf))) < 0) { - continue; - } - - for (int i = 0; i < nread; i++) { - if (syslink_parse_char(&state, buf[i], &msg)) { - handle_message(&msg); - } - } - } - - if (fds[1].revents & POLLIN) { - parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - update_params(false); - } - } - - } - - close(_fd); -} - -void -Syslink::handle_message(syslink_message_t *msg) -{ - hrt_abstime t = hrt_absolute_time(); - - if (t - _lasttime > 1000000) { - pktrate = _count; - rxrate = _count_in; - txrate = _count_out; - nullrate = _null_count; - - _lasttime = t; - _count = 0; - _null_count = 0; - _count_in = 0; - _count_out = 0; - } - - _count++; - - if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) { - // When the power button is hit - } else if (msg->type == SYSLINK_PM_BATTERY_STATE) { - - if (msg->length != 9) { - return; - } - - uint8_t flags = msg->data[0]; - int charging = flags & 1; - int powered = flags & 2; - - float vbat; //, iset; - memcpy(&vbat, &msg->data[1], sizeof(float)); - //memcpy(&iset, &msg->data[5], sizeof(float)); - - _battery.updateBatteryStatus(t, vbat, -1, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); - - - // Update battery charge state - if (charging) { - _bstate = BAT_CHARGING; - } - - /* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */ - else if (powered && !charging && vbat > 3.7f) { - _bstate = BAT_CHARGED; - - } else { - _bstate = BAT_DISCHARGING; - } - - } 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_RAW) { - handle_raw(msg); - _lastrxtime = t; - - } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) { - handle_radio(msg); - - } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) { - memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t)); - px4_sem_post(&memory_sem); - - } else { - PX4_INFO("GOT %d", msg->type); - } - - //Send queued messages - if (!_queue.empty()) { - _queue.get(msg, sizeof(syslink_message_t)); - send_message(msg); - } - - - float p = (t % 500000) / 500000.0f; - - /* Use LED_GREEN for charging indicator */ - if (_bstate == BAT_CHARGED) { - led_on(LED_GREEN); - - } else if (_bstate == BAT_CHARGING && p < 0.25f) { - led_on(LED_GREEN); - - } else { - led_off(LED_GREEN); - } - - /* Alternate RX/TX LEDS when transfering */ - bool rx = t - _lastrxtime < 200000, - tx = t - _lasttxtime < 200000; - - - if (rx && p < 0.25f) { - led_on(LED_RX); - - } else { - led_off(LED_RX); - } - - if (tx && p > 0.5f && p > 0.75f) { - led_on(LED_TX); - - } else { - led_off(LED_TX); - } - - - // resend parameters if they haven't been acknowledged - if (_params_ack[0] == 0 && t - _params_update[0] > 10000) { - set_channel(_channel); - - } else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) { - set_datarate(_rate); - - } else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) { - set_address(_addr); - } -} - -void -Syslink::handle_radio(syslink_message_t *sys) -{ - hrt_abstime t = hrt_absolute_time(); - - // record acknowlegements to parameter messages - if (sys->type == SYSLINK_RADIO_CHANNEL) { - _params_ack[0] = t; - - } else if (sys->type == SYSLINK_RADIO_DATARATE) { - _params_ack[1] = t; - - } else if (sys->type == SYSLINK_RADIO_ADDRESS) { - _params_ack[2] = t; - } -} - -void -Syslink::handle_raw(syslink_message_t *sys) -{ - crtp_message_t *c = (crtp_message_t *) &sys->length; - - if (CRTP_NULL(*c)) { - if (c->size >= 3) { - handle_bootloader(sys); - } - - _null_count++; - - } else if (c->port == CRTP_PORT_COMMANDER) { - - crtp_commander *cmd = (crtp_commander *) &c->data[0]; - - input_rc_s rc = {}; - - rc.timestamp = hrt_absolute_time(); - rc.timestamp_last_signal = rc.timestamp; - rc.channel_count = 5; - rc.rc_failsafe = false; - rc.rc_lost = false; - rc.rc_lost_frame_count = 0; - rc.rc_total_frame_count = 1; - rc.rc_ppm_frame_length = 0; - rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; - rc.rssi = _rssi; - - - double pitch = cmd->pitch, roll = cmd->roll, yaw = cmd->yaw; - - /* channels (scaled to rc limits) */ - rc.values[0] = pitch * 500 / 20 + 1500; - rc.values[1] = roll * 500 / 20 + 1500; - rc.values[2] = yaw * 500 / 150 + 1500; - rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000; - rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 - - _rc_pub.publish(rc); - - } else if (c->port == CRTP_PORT_MAVLINK) { - _count_in++; - /* Pipe to Mavlink bridge */ - _bridge->pipe_message(c); - - } else { - handle_raw_other(sys); - } - - // Block all non-requested messaged in bootloader mode - if (_bootloader_mode) { - return; - } - - // Allow one raw message to be sent from the queue - send_queued_raw_message(); -} - -void -Syslink::handle_bootloader(syslink_message_t *sys) -{ - // Minimal bootloader emulation for being detectable - // To the bitcraze utilities, the STM32 will appear to have no flashable pages - // Upon receiving a bootloader message, all outbound packets are blocked in 'bootloader mode' due to how fragile the aforementioned utilities are to extra packets - - crtp_message_t *c = (crtp_message_t *) &sys->length; - - uint8_t target = c->data[0]; - uint8_t cmd = c->data[1]; - - if (target != 0xFF) { // CF2 STM32 target - return; - } - - _bootloader_mode = true; - - if (cmd == 0x10) { // GET_INFO - - c->size = 1 + 23; - memset(&c->data[2], 0, 21); - c->data[22] = 0x10; // Protocol version - send_message(sys); - } -} - -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 + 2; - 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 (size_t 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); - 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; -} - - -namespace syslink -{ - -void usage(); -void start(); -void status(); -void test(); -void attached(int pid); - -void usage() -{ - warnx("missing command: try 'start', 'status', 'attached', 'test'"); -} - -void start() -{ - if (g_syslink != nullptr) { - printf("Already started\n"); - exit(1); - } - - g_syslink = new Syslink(); - g_syslink->start(); - - // Wait for task and bridge to start - usleep(5000); - - - warnx("Started syslink on /dev/ttyS2\n"); - exit(0); - -} - -void status() -{ - if (g_syslink == nullptr) { - printf("Please start syslink first\n"); - exit(1); - } - - printf("Connection activity:\n"); - printf("- total rx: %d p/s\n", g_syslink->pktrate); - printf("- radio rx: %d p/s (%d null)\n", g_syslink->rxrate, g_syslink->nullrate); - printf("- radio tx: %d p/s\n\n", g_syslink->txrate); - - printf("Parameter Status:\n"); - const char *goodParam = "good"; - const char *badParam = "fail!"; - printf("- channel: %s\n", g_syslink->is_good(0) ? goodParam : badParam); - printf("- data rate: %s\n", g_syslink->is_good(1) != 0 ? goodParam : badParam); - printf("- address: %s\n\n", g_syslink->is_good(2) != 0 ? goodParam : badParam); - - - int deckfd = open(DECK_DEVICE_PATH, O_RDONLY); - int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks); - printf("Deck scan: (found %d)\n", ndecks); - - for (int i = 0; i < ndecks; i++) { - ioctl(deckfd, DECKIOSNUM, (unsigned long) &i); - - uint8_t *id; - int idlen = ioctl(deckfd, DECKIOID, (unsigned long) &id); - - // TODO: Validate the ID - printf("%i: ROM ID: ", i); - - for (int idi = 0; idi < idlen; idi++) { - printf("%02X", id[idi]); - } - - deck_descriptor_t desc; - read(deckfd, &desc, sizeof(desc)); - - printf(", VID: %02X , PID: %02X\n", desc.header, desc.vendorId, desc.productId); - - // Print pages of memory - for (size_t di = 0; di < sizeof(desc); di++) { - if (di % 16 == 0) { - printf("\n"); - } - - printf("%02X ", ((uint8_t *)&desc)[di]); - - } - - printf("\n\n"); - } - - close(deckfd); - exit(0); -} - -void attached(int pid) -{ - bool found = false; - - int deckfd = open(DECK_DEVICE_PATH, O_RDONLY); - int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks); - - for (int i = 0; i < ndecks; i++) { - ioctl(deckfd, DECKIOSNUM, (unsigned long) &i); - - deck_descriptor_t desc; - read(deckfd, &desc, sizeof(desc)); - - if (desc.productId == pid) { - found = true; - break; - } - } - - close(deckfd); - exit(found ? 1 : 0); -} - -void test() -{ - // TODO: Ensure battery messages are recent - // TODO: Read and write from memory to ensure it is working -} - -} // namespace syslink - -int syslink_main(int argc, char *argv[]) -{ - if (argc < 2) { - syslink::usage(); - exit(1); - } - - const char *verb = argv[1]; - - if (!strcmp(verb, "start")) { - syslink::start(); - } - - if (!strcmp(verb, "status")) { - syslink::status(); - } - - if (!strcmp(verb, "attached")) { - if (argc != 3) { - errx(1, "usage: syslink attached [deck_pid]"); - } - - int pid = atoi(argv[2]); - syslink::attached(pid); - } - - if (!strcmp(verb, "test")) { - syslink::test(); - } - - syslink::usage(); - exit(1); - - return 0; -} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_main.h b/boards/bitcraze/crazyflie21/syslink/syslink_main.h deleted file mode 100644 index 875aa0347d..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink_main.h +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#include - -#include - -#include -#include "ringbuffer.h" - -#include -#include -#include -#include - -#include "syslink.h" -#include "crtp.h" - -using namespace time_literals; - -#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) - -typedef enum { - BAT_DISCHARGING = 0, - BAT_CHARGING = 1, - BAT_CHARGED = 2 -} battery_state; - - -class SyslinkBridge; -class SyslinkMemory; - -class Syslink -{ -public: - Syslink(); - - int start(); - - int set_datarate(uint8_t datarate); - int set_channel(uint8_t channel); - int set_address(uint64_t addr); - - int is_good(int i) { return _params_ack[i] != 0; } - - int pktrate; - int nullrate; - int rxrate; - int txrate; - -private: - friend class SyslinkBridge; - friend class SyslinkMemory; - - int open_serial(const char *dev); - - void handle_message(syslink_message_t *msg); - void handle_raw(syslink_message_t *sys); - void handle_radio(syslink_message_t *sys); - void handle_bootloader(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(); - - void update_params(bool force_set); - - int _syslink_task; - bool _task_running; - bool _bootloader_mode; - - int _count; - int _null_count; - int _count_in; - int _count_out; - hrt_abstime _lasttime; - hrt_abstime _lasttxtime; // Last time a radio message was sent - hrt_abstime _lastrxtime; // Last time a radio message was recieved - - int _fd; - - // For receiving raw syslink messages to send from other processes - ringbuffer::RingBuffer _queue; - - // Stores data that was needs to be written as a raw message - ringbuffer::RingBuffer _writebuffer; - SyslinkBridge *_bridge; - SyslinkMemory *_memory; - - int _params_sub; - - // Current parameter values - int32_t _channel, _rate; - uint64_t _addr; - 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 - - uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - - // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS - static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; - Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US}; - - int32_t _rssi; - battery_state _bstate; - - px4_sem_t memory_sem; - - syslink_message_t memory_msg; - - static int task_main_trampoline(int argc, char *argv[]); - - void task_main(); -}; - - -class SyslinkBridge : public cdev::CDev -{ - -public: - SyslinkBridge(Syslink *link); - virtual ~SyslinkBridge() = default; - - 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; - - crtp_message_t _msg_to_send; - int _msg_to_send_size_remaining; -}; - - -class SyslinkMemory : public cdev::CDev -{ - -public: - SyslinkMemory(Syslink *link); - virtual ~SyslinkMemory() = default; - - 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/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp deleted file mode 100644 index 4473339557..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/**************************************************************************** - * - * 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" - -#include - - -SyslinkMemory::SyslinkMemory(Syslink *link) : - CDev(DECK_DEVICE_PATH), - _link(link), - _activeI(0) -{ -} - - -int -SyslinkMemory::init() -{ - int ret = CDev::init(); - - /* if init failed, bail now */ - if (ret != OK) { - PX4_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(); - return 0; - - case DECKIOSNUM: - _activeI = *((int *) arg); - return 0; - - case DECKIOID: { - syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data; - getinfo(_activeI); - *((uint8_t **)arg) = data->id; - return 8; - } - - 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_GETINFO; - 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; - addr += n; - } - - return nread; -} - -int -SyslinkMemory::write(int i, uint16_t addr, const char *buf, int length) -{ - // TODO: Unimplemented - return -1; -} - -void -SyslinkMemory::sendAndWait() -{ - // TODO: Force the syslink thread to wake up - _link->_queue.force(&msgbuf, sizeof(msgbuf)); - px4_sem_wait(&_link->memory_sem); -} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_params.c b/boards/bitcraze/crazyflie21/syslink/syslink_params.c deleted file mode 100644 index 921ec014ae..0000000000 --- a/boards/bitcraze/crazyflie21/syslink/syslink_params.c +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * 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_params.c - * - * Parameters defined by the syslink module and the exposed NRF51 radio - * - * @author Dennis Shtatnov - */ - -/** - * Operating channel of the NRF51 - * - * @min 0 - * @max 125 - * @group Syslink - */ -PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80); - -/** - * Operating datarate of the NRF51 - * - * @min 0 - * @max 2 - * @group Syslink - */ -PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 2); - -/** - * Operating address of the NRF51 (most significant byte) - * - * @group Syslink - */ -PARAM_DEFINE_INT32(SLNK_RADIO_ADDR1, 231); // 0xE7 - -/** - * Operating address of the NRF51 (least significant 4 bytes) - * - * @group Syslink - */ -PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7