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

Syslink memory/deck interface

This commit is contained in:
Dennis Shtatnov 2016-09-16 21:48:10 -04:00 committed by Lorenz Meier
parent 83105fca95
commit 937f3477d3
7 changed files with 338 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,6 +37,7 @@ px4_add_module(
SRCS
syslink_main.cpp
syslink_bridge.cpp
syslink_memory.cpp
syslink_params.c
syslink.c
DEPENDS

79
src/modules/syslink/drv_deck.h Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
*
* Copyright (C) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* Definitions for crazyflie related drivers */
#ifndef _DRV_CRAZYFLIE_H
#define _DRV_CRAZYFLIE_H
#include <px4_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;
uint32_t pins;
uint8_t vendorId;
uint8_t productId;
uint8_t crc;
uint8_t data[104];
} deck_descriptor_t;
/*
* ioctl() definitions
*/
#define _DECKIOCBASE (0x4100)
#define _DECKIOC(_n) (_PX4_IOC(_DECKIOCBASE, _n))
/** get the number of connected deck memory devices */
#define DECKIOGNUM _DECKIOC(0)
/** set the index of the current deck memory being accessed */
#define DECKIOSNUM _DECKIOC(1)
#define DECKIOID _DECKIOC(2)
#endif

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

@ -37,7 +37,9 @@
extern const char *syslink_magic;
#define SYSLINK_GROUP 0xF0
#define SYSLINK_RADIO 0x00
#define SYSLINK_RADIO_RAW 0x00
#define SYSLINK_RADIO_CHANNEL 0x01
#define SYSLINK_RADIO_DATARATE 0x02
@ -45,12 +47,14 @@ extern const char *syslink_magic;
#define SYSLINK_RADIO_RSSI 0x04
#define SYSLINK_RADIO_ADDRESS 0x05
#define SYSLINK_PM 0x10
#define SYSLINK_PM_SOURCE 0x10
#define SYSLINK_PM_ONOFF_SWITCHOFF 0x11
#define SYSLINK_PM_BATTERY_VOLTAGE 0x12
#define SYSLINK_PM_BATTERY_STATE 0x13
#define SYSLINK_PM_BATTERY_AUTOUPDATE 0x14
#define SYSLINK_OW 0x20
#define SYSLINK_OW_SCAN 0x20
#define SYSLINK_OW_GETINFO 0x21
#define SYSLINK_OW_READ 0x22
@ -71,6 +75,34 @@ typedef struct {
uint8_t cksum[2];
} syslink_message_t;
#define OW_SIZE 112
#define OW_READ_BLOCK 29
#define OW_WRITE_BLOCK 27
typedef struct {
uint8_t nmems;
} syslink_ow_scan_t;
typedef struct {
uint8_t idx;
uint8_t id;
} syslink_ow_getinfo_t;
typedef struct {
uint8_t idx;
uint16_t addr;
uint8_t data[OW_READ_BLOCK];
} syslink_ow_read_t;
typedef struct {
uint8_t idx;
uint16_t addr;
uint16_t length;
uint8_t data[OW_WRITE_BLOCK];
} syslink_ow_write_t;
typedef enum {
SYSLINK_STATE_START = 0,
SYSLINK_STATE_TYPE,

4
src/modules/syslink/syslink_bridge.cpp
View File

@ -88,8 +88,6 @@ SyslinkBridge::poll_state(struct file *filp)
return 0;
}
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
ssize_t
SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen)
{
@ -132,8 +130,6 @@ SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen)
return buflen;
}
int
SyslinkBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{

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

@ -87,7 +87,8 @@ Syslink::Syslink() :
_rssi(RC_INPUT_RSSI_MAX),
_bstate(BAT_DISCHARGING)
{
px4_sem_init(&radio_sem, 0, 0);
px4_sem_init(&memory_sem, 0, 0);
}
@ -238,6 +239,9 @@ Syslink::task_main()
_bridge = new SyslinkBridge(this);
_bridge->init();
_memory = new SyslinkMemory(this);
_memory->init();
_battery.reset(&_battery_status);
@ -373,7 +377,7 @@ Syslink::handle_message(syslink_message_t *msg)
_bstate = BAT_CHARGED;
else
_bstate = BAT_DISCHARGING;
// announce the battery status if needed, just publish else
if (_battery_pub != nullptr) {
@ -386,16 +390,16 @@ Syslink::handle_message(syslink_message_t *msg)
} else if (msg->type == SYSLINK_RADIO_RSSI) {
uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
_rssi = 140 - rssi * 100 / (100 - 40);
} else if (msg->type == SYSLINK_RADIO_CHANNEL) {
PX4_INFO("Channel ACK %d", msg->data[0]);
} else if (msg->type == SYSLINK_RADIO_DATARATE) {
PX4_INFO("Datarate ACK %d", msg->data[0]);
} else if (msg->type == SYSLINK_RADIO_RAW) {
handle_raw(msg);
lastrxtime = t;
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
radio_msg = *msg;
px4_sem_post(&radio_sem);
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
_memory->msgbuf = *msg;
px4_sem_post(&memory_sem);
} else {
PX4_INFO("GOT %d", msg->type);
}

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

@ -43,6 +43,9 @@
#include "syslink.h"
#include "crtp.h"
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
typedef enum {
BAT_DISCHARGING = 0,
BAT_CHARGING = 1,
@ -51,6 +54,7 @@ typedef enum {
class SyslinkBridge;
class SyslinkMemory;
class Syslink
{
@ -67,6 +71,7 @@ public:
private:
friend class SyslinkBridge;
friend class SyslinkMemory;
int open_serial(const char *dev);
@ -94,6 +99,7 @@ private:
// Stores data that was needs to be written as a raw message
ringbuffer::RingBuffer _writebuffer;
SyslinkBridge *_bridge;
SyslinkMemory *_memory;
orb_advert_t _battery_pub;
orb_advert_t _rc_pub;
@ -106,6 +112,12 @@ private:
int32_t _rssi;
battery_state _bstate;
px4_sem_t radio_sem;
px4_sem_t memory_sem;
syslink_message_t radio_msg;
syslink_message_t memory_msg;
static int task_main_trampoline(int argc, char *argv[]);
void task_main();
@ -142,3 +154,36 @@ private:
};
class SyslinkMemory : public device::CDev
{
public:
SyslinkMemory(Syslink *link);
virtual ~SyslinkMemory();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
private:
friend class Syslink;
Syslink *_link;
int _activeI;
syslink_message_t msgbuf;
uint8_t scan();
void getinfo(int i);
int read(int i, uint16_t addr, char *buf, int length);
int write(int i, uint16_t addr, const char *buf, int length);
void sendAndWait();
};

168
src/modules/syslink/syslink_memory.cpp Normal file
View File

@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "syslink_main.h"
#include "drv_deck.h"
SyslinkMemory::SyslinkMemory(Syslink *link) :
CDev("SyslinkMemory", DECK_DEVICE_PATH),
_link(link),
_activeI(0)
{
}
SyslinkMemory::~SyslinkMemory()
{
}
int
SyslinkMemory::init()
{
int ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
return ret;
}
ssize_t
SyslinkMemory::read(struct file *filp, char *buffer, size_t buflen)
{
return read(_activeI, 0, buffer, buflen);
}
ssize_t
SyslinkMemory::write(struct file *filp, const char *buffer, size_t buflen)
{
// For now, unsupported
return -1;
// return buflen;
}
int
SyslinkMemory::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DECKIOGNUM:
*((int *) arg) = scan();
case DECKIOSNUM:
_activeI = *((int *) arg);
return 0;
case DECKIOID:
syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data;
getinfo(_activeI)
arg = &data->index;
return sizeof(data->index);
default:
CDev::ioctl(filp, cmd, arg);
return 0;
}
}
uint8_t
SyslinkMemory::scan()
{
syslink_ow_scan_t *data = (syslink_ow_scan_t *) &msgbuf.data;
msgbuf.type = SYSLINK_OW_SCAN;
msgbuf.length = 0;
sendAndWait();
return data->nmems;
}
void
SyslinkMemory::getinfo(int i)
{
syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data;
msgbuf.type = SYSLINK_OW_SCAN;
msgbuf.length = 1;
data->idx = i;
sendAndWait();
}
int
SyslinkMemory::read(int i, uint16_t addr, char *buf, int length)
{
syslink_ow_read_t *data = (syslink_ow_read_t *) &msgbuf.data;
msgbuf.type = SYSLINK_OW_READ;
int nread = 0;
while (nread < length) {
msgbuf.length = 3;
data->idx = i;
data->addr = addr;
sendAndWait();
// Number of bytes actually read
int n = MIN(length - nread, msgbuf.length - 3);
if(n == 0)
break;
memcpy(buf, data->data, n);
nread += n;
buf += n;
}
return nread;
}
int
SyslinkMemory::write(int i, uint16_t addr, const char *buf, int length)
{
// TODO: Unimplemented
return -1;
}
void
SyslinkMemory::sendAndWait()
{
// TODO: Mutex lock sending a message
_link->send_message(&msgbuf);
px4_sem_wait(&_link->memory_sem);
}