forked from Archive/PX4-Autopilot
Syslink cli deck detection
This commit is contained in:
Dennis Shtatnov
Lorenz Meier
parent
937f3477d3
commit
f334a6225a
|
|
@ -946,7 +946,7 @@ then
|
|||
tone_alarm error
|
||||
fi
|
||||
|
||||
else
|
||||
fi
|
||||
|
||||
# End of autostart
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Definitions for crazyflie related drivers */
|
||||
/* Definitions for crazyflie related drivers */
|
||||
|
||||
#ifndef _DRV_CRAZYFLIE_H
|
||||
#define _DRV_CRAZYFLIE_H
|
||||
|
|
@ -47,14 +47,14 @@
|
|||
|
||||
/* structure of the data stored in deck memory */
|
||||
typedef struct {
|
||||
uint8_t header;
|
||||
uint8_t header; // Should be 0xEB
|
||||
uint32_t pins;
|
||||
uint8_t vendorId;
|
||||
uint8_t productId;
|
||||
uint8_t crc;
|
||||
uint8_t data[104];
|
||||
|
||||
} deck_descriptor_t;
|
||||
} __attribute__((packed)) deck_descriptor_t;
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -73,34 +73,40 @@ typedef struct {
|
|||
uint8_t length;
|
||||
uint8_t data[SYSLINK_MAX_DATA_LEN];
|
||||
uint8_t cksum[2];
|
||||
} syslink_message_t;
|
||||
} __attribute__((packed)) syslink_message_t;
|
||||
|
||||
|
||||
#define OW_SIZE 112
|
||||
#define OW_READ_BLOCK 29
|
||||
#define OW_WRITE_BLOCK 27
|
||||
#define OW_WRITE_BLOCK 26 // TODO: Use even, but can be up to 27
|
||||
|
||||
typedef struct {
|
||||
uint8_t nmems;
|
||||
} syslink_ow_scan_t;
|
||||
} __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;
|
||||
} syslink_ow_getinfo_t;
|
||||
uint8_t id[8];
|
||||
} __attribute__((packed)) syslink_ow_getinfo_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t idx;
|
||||
uint16_t addr;
|
||||
uint8_t data[OW_READ_BLOCK];
|
||||
} syslink_ow_read_t;
|
||||
} __attribute__((packed)) 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;
|
||||
} __attribute__((packed)) syslink_ow_write_t;
|
||||
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -49,6 +49,10 @@
|
|||
#include <sys/ioctl.h>
|
||||
#include <poll.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_led.h>
|
||||
|
|
@ -63,6 +67,8 @@
|
|||
|
||||
#include "crtp.h"
|
||||
#include "syslink_main.h"
|
||||
#include "drv_deck.h"
|
||||
|
||||
|
||||
__BEGIN_DECLS
|
||||
extern void led_init(void);
|
||||
|
|
@ -74,12 +80,24 @@ __END_DECLS
|
|||
extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); }
|
||||
|
||||
|
||||
Syslink *g_syslink;
|
||||
Syslink *g_syslink = nullptr;
|
||||
|
||||
Syslink::Syslink() :
|
||||
pktrate(0),
|
||||
nullrate(0),
|
||||
rxrate(0),
|
||||
txrate(0),
|
||||
_syslink_task(-1),
|
||||
_task_running(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)),
|
||||
_battery_pub(nullptr),
|
||||
_rc_pub(nullptr),
|
||||
|
|
@ -116,7 +134,20 @@ Syslink::set_datarate(uint8_t datarate)
|
|||
msg.type = SYSLINK_RADIO_DATARATE;
|
||||
msg.length = 1;
|
||||
msg.data[0] = datarate;
|
||||
return send_message(&msg);
|
||||
|
||||
if (send_message(&msg) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Wait for a second
|
||||
// struct timespec abstime = {1, 0};
|
||||
// if(px4_sem_timedwait(&radio_sem, &abstime) != 0) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -139,11 +170,6 @@ Syslink::set_address(uint64_t addr)
|
|||
return send_message(&msg);
|
||||
}
|
||||
|
||||
|
||||
int count_out = 0;
|
||||
|
||||
static hrt_abstime lasttxtime = 0; // Last time a radio message was recieved
|
||||
|
||||
int
|
||||
Syslink::send_queued_raw_message()
|
||||
{
|
||||
|
|
@ -151,12 +177,12 @@ Syslink::send_queued_raw_message()
|
|||
return 0;
|
||||
}
|
||||
|
||||
lasttxtime = hrt_absolute_time();
|
||||
_lasttxtime = hrt_absolute_time();
|
||||
|
||||
syslink_message_t msg;
|
||||
msg.type = SYSLINK_RADIO_RAW;
|
||||
|
||||
count_out++;
|
||||
_count_out++;
|
||||
|
||||
_writebuffer.get(&msg.length, sizeof(crtp_message_t));
|
||||
|
||||
|
|
@ -323,32 +349,25 @@ Syslink::task_main()
|
|||
|
||||
}
|
||||
|
||||
|
||||
|
||||
static int count = 0;
|
||||
static int null_count = 0;
|
||||
static int count_in = 0;
|
||||
//static int count_out = 0;
|
||||
static hrt_abstime lasttime = 0;
|
||||
|
||||
static hrt_abstime lastrxtime = 0; // Last time a radio message was recieved
|
||||
|
||||
|
||||
void
|
||||
Syslink::handle_message(syslink_message_t *msg)
|
||||
{
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (t - lasttime > 1000000) {
|
||||
PX4_INFO("%d p/s (%d null) (%d in / %d out raw)", count, null_count, count_in, count_out);
|
||||
lasttime = t;
|
||||
count = 0;
|
||||
null_count = 0;
|
||||
count_in = 0;
|
||||
count_out = 0;
|
||||
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++;
|
||||
_count++;
|
||||
|
||||
if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
|
||||
// When the power button is hit
|
||||
|
|
@ -370,13 +389,17 @@ Syslink::handle_message(syslink_message_t *msg)
|
|||
|
||||
|
||||
// Update battery charge state
|
||||
if(charging)
|
||||
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 && _battery_status.voltage_filtered_v > 3.7f)
|
||||
else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
|
||||
_bstate = BAT_CHARGED;
|
||||
else
|
||||
|
||||
} else {
|
||||
_bstate = BAT_DISCHARGING;
|
||||
}
|
||||
|
||||
|
||||
// announce the battery status if needed, just publish else
|
||||
|
|
@ -390,47 +413,61 @@ 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_RAW) {
|
||||
handle_raw(msg);
|
||||
lastrxtime = t;
|
||||
_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;
|
||||
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) {
|
||||
if (_bstate == BAT_CHARGED) {
|
||||
led_on(LED_GREEN);
|
||||
}
|
||||
else if(_bstate == BAT_CHARGING && p < 0.25f) {
|
||||
|
||||
} else if (_bstate == BAT_CHARGING && p < 0.25f) {
|
||||
led_on(LED_GREEN);
|
||||
}
|
||||
else
|
||||
|
||||
} else {
|
||||
led_off(LED_GREEN);
|
||||
}
|
||||
|
||||
/* Alternate RX/TX LEDS when transfering */
|
||||
bool rx = t - lastrxtime < 200000,
|
||||
tx = t - lasttxtime < 200000;
|
||||
bool rx = t - _lastrxtime < 200000,
|
||||
tx = t - _lasttxtime < 200000;
|
||||
|
||||
|
||||
if(rx && p < 0.25f)
|
||||
if (rx && p < 0.25f) {
|
||||
led_on(LED_RX);
|
||||
else
|
||||
led_off(LED_RX);
|
||||
|
||||
if(tx && p > 0.5f && p > 0.75f)
|
||||
} else {
|
||||
led_off(LED_RX);
|
||||
}
|
||||
|
||||
if (tx && p > 0.5f && p > 0.75f) {
|
||||
led_on(LED_TX);
|
||||
else
|
||||
|
||||
} else {
|
||||
led_off(LED_TX);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -442,7 +479,7 @@ Syslink::handle_raw(syslink_message_t *sys)
|
|||
if (CRTP_NULL(*c)) {
|
||||
// TODO: Handle bootloader messages if possible
|
||||
|
||||
null_count++;
|
||||
_null_count++;
|
||||
|
||||
} else if (c->port == CRTP_PORT_COMMANDER) {
|
||||
|
||||
|
|
@ -479,7 +516,7 @@ Syslink::handle_raw(syslink_message_t *sys)
|
|||
}
|
||||
|
||||
} else if (c->port == CRTP_PORT_MAVLINK) {
|
||||
count_in++;
|
||||
_count_in++;
|
||||
/* Pipe to Mavlink bridge */
|
||||
_bridge->pipe_message(c);
|
||||
|
||||
|
|
@ -599,16 +636,165 @@ Syslink::send_message(syslink_message_t *msg)
|
|||
}
|
||||
|
||||
|
||||
|
||||
int syslink_main(int argc, char *argv[])
|
||||
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);
|
||||
|
||||
PX4_INFO("Started syslink on /dev/ttyS2");
|
||||
|
||||
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);
|
||||
|
||||
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 (int 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
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -68,6 +68,12 @@ public:
|
|||
int set_channel(uint8_t channel);
|
||||
int set_address(uint64_t addr);
|
||||
|
||||
|
||||
int pktrate;
|
||||
int nullrate;
|
||||
int rxrate;
|
||||
int txrate;
|
||||
|
||||
private:
|
||||
|
||||
friend class SyslinkBridge;
|
||||
|
|
@ -94,8 +100,19 @@ private:
|
|||
int _syslink_task;
|
||||
bool _task_running;
|
||||
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -87,15 +87,18 @@ 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)
|
||||
arg = &data->index;
|
||||
return sizeof(data->index);
|
||||
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);
|
||||
|
|
@ -119,7 +122,7 @@ void
|
|||
SyslinkMemory::getinfo(int i)
|
||||
{
|
||||
syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data;
|
||||
msgbuf.type = SYSLINK_OW_SCAN;
|
||||
msgbuf.type = SYSLINK_OW_GETINFO;
|
||||
msgbuf.length = 1;
|
||||
data->idx = i;
|
||||
sendAndWait();
|
||||
|
|
@ -132,6 +135,7 @@ SyslinkMemory::read(int i, uint16_t addr, char *buf, int length)
|
|||
msgbuf.type = SYSLINK_OW_READ;
|
||||
|
||||
int nread = 0;
|
||||
|
||||
while (nread < length) {
|
||||
|
||||
msgbuf.length = 3;
|
||||
|
|
@ -141,12 +145,15 @@ SyslinkMemory::read(int i, uint16_t addr, char *buf, int length)
|
|||
|
||||
// Number of bytes actually read
|
||||
int n = MIN(length - nread, msgbuf.length - 3);
|
||||
if(n == 0)
|
||||
|
||||
if (n == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
memcpy(buf, data->data, n);
|
||||
nread += n;
|
||||
buf += n;
|
||||
addr += n;
|
||||
}
|
||||
|
||||
return nread;
|
||||
|
|
@ -162,7 +169,7 @@ SyslinkMemory::write(int i, uint16_t addr, const char *buf, int length)
|
|||
void
|
||||
SyslinkMemory::sendAndWait()
|
||||
{
|
||||
// TODO: Mutex lock sending a message
|
||||
_link->send_message(&msgbuf);
|
||||
// TODO: Force the syslink thread to wake up
|
||||
_link->_queue.force(&msgbuf, sizeof(msgbuf));
|
||||
px4_sem_wait(&_link->memory_sem);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue