forked from Archive/PX4-Autopilot
[UPDATE] - Remove duplicate syslink code
This commit is contained in:
parent
641665f2e9
commit
dd9b0ded0f
|
@ -31,4 +31,4 @@
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
add_subdirectory(syslink)
|
add_subdirectory(../crazyflie syslink)
|
||||||
|
|
|
@ -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
|
|
||||||
)
|
|
|
@ -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 <stdint.h>
|
|
||||||
|
|
||||||
#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;
|
|
|
@ -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 <px4_platform_common/defines.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <sys/ioctl.h>
|
|
||||||
|
|
||||||
|
|
||||||
#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
|
|
|
@ -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 <string.h>
|
|
||||||
|
|
||||||
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
|
|
|
@ -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 <stddef.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
|
|
||||||
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
|
|
|
@ -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 <densht@gmail.com>
|
|
||||||
*/
|
|
||||||
|
|
||||||
|
|
||||||
#include <px4_platform_common/defines.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <sys/ioctl.h>
|
|
||||||
#include <systemlib/err.h>
|
|
||||||
#include <poll.h>
|
|
||||||
#include <termios.h>
|
|
||||||
|
|
||||||
|
|
||||||
#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;
|
|
||||||
}
|
|
|
@ -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 <stdint.h>
|
|
||||||
|
|
||||||
|
|
||||||
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
|
|
|
@ -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 <cstring>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
|
@ -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 <px4_platform_common/px4_config.h>
|
|
||||||
#include <px4_platform_common/tasks.h>
|
|
||||||
#include <px4_platform_common/posix.h>
|
|
||||||
#include <px4_platform_common/defines.h>
|
|
||||||
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <poll.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <sys/ioctl.h>
|
|
||||||
#include <termios.h>
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <mqueue.h>
|
|
||||||
|
|
||||||
#include <drivers/drv_rc_input.h>
|
|
||||||
#include <drivers/drv_board_led.h>
|
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
|
||||||
|
|
||||||
#include <board_config.h>
|
|
||||||
|
|
||||||
#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;
|
|
||||||
}
|
|
|
@ -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 <stdint.h>
|
|
||||||
|
|
||||||
#include <battery/battery.h>
|
|
||||||
|
|
||||||
#include <drivers/device/device.h>
|
|
||||||
#include "ringbuffer.h"
|
|
||||||
|
|
||||||
#include <uORB/PublicationMulti.hpp>
|
|
||||||
#include <uORB/topics/parameter_update.h>
|
|
||||||
#include <uORB/topics/battery_status.h>
|
|
||||||
#include <uORB/topics/input_rc.h>
|
|
||||||
|
|
||||||
#include "syslink.h"
|
|
||||||
#include "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<input_rc_s> _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();
|
|
||||||
};
|
|
|
@ -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 <cstring>
|
|
||||||
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
|
@ -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 <densht@gmail.com>
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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
|
|
Loading…
Reference in New Issue