Inertial commit

main.cpp

@@ -0,0 +1,153 @@
+#include <Arduino.h>
+
+#include "helper/SimpleRingBuffer.h"
+#include "helper/DleEncoder.h"
+#include "helper/crc_ccitt.h"
+
+//SPI is formally a library, so it is not part of the objects compiled
+//from the core and we need to include it explicitly
+#include "arduino_core/ArduinoCore-avr/libraries/SPI/src/SPI.h"
+
+//Define which port to use for the SPI Chip Select
+#define CS_PORT PORTC
+#define CS_DDR DDRC
+
+#define RING_BUFFER_SIZE 100
+#define MAX_PACKET_LENGTH 20
+
+static const uint8_t COMMAND_TRANSFER_SPI = 1;
+
+SimpleRingBuffer ringBuffer(RING_BUFFER_SIZE, true);
+
+uint8_t rawData[2 * RING_BUFFER_SIZE];
+size_t rawDataSize = 0;
+
+/**
+ * Encode and send the data, last two bytes of data are assumed to be for the checksum
+ * and will be overwritten with it
+ */
+void sendData(uint8_t *data, size_t len) {
+	uint16_t crc = Calculate_CRC(data,len-2);
+	data[len-2] = crc >> 8;
+	data[len-1] = crc;
+	//we're being conservative here
+	//TODO move this to global so other protocols can use it too
+	uint8_t buffer[2*len +2];
+	buffer[0] = DleEncoder::STX;
+	size_t writtenLen;
+	ReturnValue_t result = DleEncoder::encode(data,len,buffer,sizeof(buffer),&writtenLen, true);
+	if (result != HasReturnvaluesIF::RETURN_OK){
+		return;
+	}
+	Serial.write(buffer, writtenLen);
+}
+
+void transferSPI(uint8_t address, uint8_t *data, size_t datalen) {
+	SPI.beginTransaction(SPISettings(14000000, MSBFIRST, SPI_MODE0));
+	CS_PORT = ~address;
+	SPI.transfer(data,datalen);
+	delay(100);
+	CS_PORT = 0xff;
+	SPI.endTransaction();
+
+}
+
+void handlePacket(uint8_t *packet, size_t packetLen) {
+	//Paket layout is:
+	// 8 bit command | 8 bit address | 16bit length | <length> byte data | 16 bit crc
+
+	uint16_t crc = Calculate_CRC(packet, packetLen);
+
+	if (crc != 0) {
+		//Serial.println("invalid Checksum");
+		return;
+	}
+
+
+	uint16_t payloadLen = (packet[2] << 8) | packet[3];
+
+	if (payloadLen + 6 != packetLen) {
+		//Serial.println("invalid len");
+		return;
+	}
+
+	uint8_t command = packet[0];
+	uint8_t address = packet[1];
+
+	switch (command) {
+	case COMMAND_TRANSFER_SPI:
+		transferSPI(address, packet + 4, payloadLen);
+		//echo the data back, no need to change the header fields, they are the same
+		//checksum will be written by sendData()
+		sendData(packet,packetLen);
+		break;
+	default:
+		//Serial.println("invalid command");
+		break;
+	}
+
+}
+
+void handleNewData() {
+	ringBuffer.readData(rawData, sizeof(rawData), true, &rawDataSize);
+
+	if (rawDataSize == 0) {
+		return;
+	}
+
+	//look for STX
+	size_t firstSTXinRawData = 0;
+	while ((firstSTXinRawData < rawDataSize)
+			&& (rawData[firstSTXinRawData] != DleEncoder::STX)) {
+		firstSTXinRawData++;
+	}
+
+	if (rawData[firstSTXinRawData] != DleEncoder::STX) {
+		//there is no STX in our data, throw it away...
+		Serial.println(firstSTXinRawData);
+		ringBuffer.deleteData(rawDataSize);
+		return;
+	}
+
+	uint8_t packet[MAX_PACKET_LENGTH];
+	size_t packetLen;
+
+	size_t readSize;
+
+	ReturnValue_t result = DleEncoder::decode(rawData + firstSTXinRawData,
+			rawDataSize - firstSTXinRawData, &readSize, packet, sizeof(packet),
+			&packetLen);
+
+	size_t toDelete = firstSTXinRawData;
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		handlePacket(packet, packetLen);
+
+		//after handling the packet, we can delete it from the raw stream, it has been copied to packet
+		toDelete += readSize;
+	}
+
+	//remove Data which was processed
+	ringBuffer.deleteData(toDelete);
+
+}
+
+//TODO check if this is thread safe by arduino
+void serialEvent() {
+	while (Serial.available()) {
+		uint8_t byte = Serial.read();
+		ringBuffer.writeData(&byte, 1);
+	}
+}
+
+void setup() {
+	CS_DDR = 0xff;
+	CS_PORT = 0xff;
+	Serial.begin(9600);
+	SPI.begin();
+	pinMode(LED_BUILTIN, OUTPUT);
+}
+
+void loop() {
+	handleNewData();
+	delay(100);
+}