Syslink cli deck detection

ROMFS/px4fmu_common/init.d/rcS

@@ -946,7 +946,7 @@ then
 			tone_alarm error
 		fi
 
-	else
+	fi
 
 # End of autostart
 fi

src/modules/syslink/drv_deck.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;
 
 
 

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;
+	uint8_t id[8];
-} syslink_ow_getinfo_t;
+} __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 {

src/modules/syslink/syslink_main.cpp

@@ -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) {
+	if (t - _lasttime > 1000000) {
-		PX4_INFO("%d p/s (%d null) (%d in / %d out raw)", count, null_count, count_in, count_out);
+		pktrate = _count;
-		lasttime = t;
+		rxrate = _count_in;
-		count = 0;
+		txrate = _count_out;
-		null_count = 0;
+		nullrate = _null_count;
-		count_in = 0;
+
-		count_out = 0;
+		_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) {
 		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,
+	bool rx = t - _lastrxtime < 200000,
-		 tx = t - lasttxtime < 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)
 }
 
 
-
+namespace syslink
-int syslink_main(int argc, char *argv[])
 {
+
+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;
 }

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;

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:
+	case DECKIOID: {
 			syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data;
-		getinfo(_activeI)
+			getinfo(_activeI);
-		arg = &data->index;
+			*((uint8_t **)arg) = data->id;
-		return sizeof(data->index);
+			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
+	// TODO: Force the syslink thread to wake up
-	_link->send_message(&msgbuf);
+	_link->_queue.force(&msgbuf, sizeof(msgbuf));
 	px4_sem_wait(&_link->memory_sem);
 }