1 files changed, 110 insertions, 0 deletions
diff --git a/Venus_Skeleton/comm.ino b/Venus_Skeleton/comm.ino
new file mode 100644
index 0000000..d60138e
--- /dev/null
+++ b/Venus_Skeleton/comm.ino
@@ -0,0 +1,110 @@
+/**
+ * Serial communication module.
+ */
+
+#include <stdbool.h>
+#include "definitions.h"
+#include "dataTypes.h"
+#include "comm.h"
+
+serial_state_t serial_state = SERIAL_UNKNOWN;
+
+static void handle_recv(int r);
+
+static void handle_ctrl(int r) {
+	// detect reset procedure
+	switch (r) {
+	case RESET1:
+		// reset request received, ack and wait for RESET2
+		Serial.write(ACK1);
+		serial_state = SERIAL_INIT;
+		return;
+	case RESET2:
+		if (serial_state == SERIAL_INIT) {
+			// Handshake completed!
+			Serial.write(ACK2);
+			serial_state = SERIAL_READY;
+		} else {
+			// invalid state, reject it
+			Serial.write(OUTOFSYNC);
+			serial_state = SERIAL_UNKNOWN;
+		}
+		return;
+	case PONG:
+		// TODO mark as alive if PING was previously sent
+		return;
+	}
+	// TODO commands for navigation? Use DATA_LEN, etc?
+}
+
+void handle_serial(data_t *data, int changedBits) {
+	// first attempt sync
+	while (Serial.available()) {
+		int r = Serial.read();
+
+		if ((r & 0xC0) == 0x80) {
+			// handle control packets
+			handle_ctrl(r);
+		// TODO if DATA_ESCAPE or DATA_LEN1, react appropriately
+		} else if (serial_state == SERIAL_READY) {
+			// handle normal data only when the handshake has completed.
+			handle_recv(r);
+		}
+	}
+
+	if (serial_state != SERIAL_READY) {
+		// Hardware is not ready, do not send data.
+		return;
+	}
+
+	// then send queued data
+}
+
+// writes data of length len
+static void serial_write_len(unsigned len) {
+	len--; // length on wire is one smaller (00 means 1 byte, 01 means 2, etc.)
+	if (len < 56) {
+		Serial.write(0x80 + len + 7);
+	} else if (len < DATA_MAX_SIZE) {
+		Serial.write(0x88 | (len >> 5));
+		Serial.write(0xA0 | (len & 0x1F));
+	} else {
+		// zero length or too long.
+	}
+}
+
+// write remainder of data
+static void serial_write_data(const char *data, unsigned len) {
+	for (unsigned i = 0; i < len; i++) {
+		char c = data[i];
+		if (c & 0x80) {
+			Serial.write(DATA_ESCAPE);
+			Serial.write(c & ~0x80);
+		} else {
+			Serial.write(c);
+		}
+	}
+}
+
+void serial_print_debug(const char *str) {
+	unsigned len = strlen(str);
+	const char c = DATA_DEBUG_PREFIX;
+
+	// only write data when the serial connection is ready
+	if (serial_state != SERIAL_READY) {
+		return;
+	}
+
+	while (len > 0) {
+		// len is no more than DATA_MAX_SIZE - 1 because of debug prefix
+		unsigned datalen = len >= DATA_MAX_SIZE ? DATA_MAX_SIZE - 1 : len;
+		// add one for the debug prefix
+		serial_write_len(1 + datalen);
+		serial_write_data(&c, 1);         /* write prefix */
+		serial_write_data(str, datalen);  /* write actual data payload */
+		len -= datalen;
+	}
+}
+
+static void handle_recv(int r) {
+}