diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22f3320f23..848fce0f0b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -946,7 +946,7 @@ then tone_alarm error fi - else + fi # End of autostart fi diff --git a/src/modules/syslink/drv_deck.h b/src/modules/syslink/drv_deck.h index 76a9500f25..adb7a13b29 100644 --- a/src/modules/syslink/drv_deck.h +++ b/src/modules/syslink/drv_deck.h @@ -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; diff --git a/src/modules/syslink/syslink.h b/src/modules/syslink/syslink.h index 9f23060dda..7261c56574 100644 --- a/src/modules/syslink/syslink.h +++ b/src/modules/syslink/syslink.h @@ -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 { diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 9c511d6911..fc3e224060 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -49,6 +49,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -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; } diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h index 0b8d3b5838..1e04b1185c 100644 --- a/src/modules/syslink/syslink_main.h +++ b/src/modules/syslink/syslink_main.h @@ -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; diff --git a/src/modules/syslink/syslink_memory.cpp b/src/modules/syslink/syslink_memory.cpp index 4d6f6155d0..eb5ef863a1 100644 --- a/src/modules/syslink/syslink_memory.cpp +++ b/src/modules/syslink/syslink_memory.cpp @@ -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); }