diff --git a/crossbow.ino b/crossbow.ino index 267b459..1d3cad4 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,11 +1,14 @@ #include #include "variables.h" +#include "qsp.h" // #define LORA_HARDWARE_SERIAL #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX + +int ppm[PPM_CHANNEL_COUNT] = {0}; /* * Main defines for device working in TX mode @@ -29,8 +32,6 @@ bool canTransmit = true; #define OLED_RESET 4 Adafruit_SSD1306 display(OLED_RESET); -int ppm[PPM_CHANNEL_COUNT] = {0}; - bool canTransmit = false; #endif @@ -39,88 +40,63 @@ bool canTransmit = false; * Start of QSP protocol implementation */ static uint8_t protocolState = IDLE; -static uint8_t packetId = 0; static uint8_t qspCrc = 0; static uint8_t qspPayload[QSP_PAYLOAD_LENGTH] = {0}; static uint8_t qspPayloadLength = 0; static uint8_t qspFrameToSend = 0; -uint8_t qspGetPacketId() { +uint8_t qspGetPacketId() +{ + static uint8_t packetId = 0; + return packetId++; } -void qspClearPayload() { - for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) { +void qspClearPayload() +{ + for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) + { qspPayload[i] = 0; } qspPayloadLength = 0; } -void qspDecodeRcDataFrame() { - int temporaryPpmOutput[PPM_CHANNEL_COUNT] = {0}; - //TODO fix it, baby :) - - temporaryPpmOutput[0] = (uint16_t) (((uint16_t) qspPayload[0] << 2) & 0x3fc) | ((qspPayload[1] >> 6) & 0x03); - temporaryPpmOutput[1] = (uint16_t) (((uint16_t) qspPayload[1] << 4) & 0x3f0) | ((qspPayload[2] >> 4) & 0x0F); - temporaryPpmOutput[2] = (uint16_t) (((uint16_t) qspPayload[2] << 6) & 0x3c0) | ((qspPayload[3] >> 2) & 0x3F); - temporaryPpmOutput[3] = (uint16_t) (((uint16_t) qspPayload[3] << 8) & 0x300) | ((qspPayload[4] >> 2) & 0xFF); - temporaryPpmOutput[4] = qspPayload[5]; - temporaryPpmOutput[5] = qspPayload[6]; - temporaryPpmOutput[6] = (qspPayload[7] >> 4) & 0b00001111; - temporaryPpmOutput[7] = qspPayload[7] & 0b00001111; - temporaryPpmOutput[8] = (qspPayload[8] >> 4) & 0b00001111; - temporaryPpmOutput[9] = qspPayload[8] & 0b00001111; - - //10bit channels - temporaryPpmOutput[0] = map(temporaryPpmOutput[0], 0, 1000, 1000, 2000); - temporaryPpmOutput[1] = map(temporaryPpmOutput[1], 0, 1000, 1000, 2000); - temporaryPpmOutput[2] = map(temporaryPpmOutput[2], 0, 1000, 1000, 2000); - temporaryPpmOutput[3] = map(temporaryPpmOutput[3], 0, 1000, 1000, 2000); - - //8bit channels - temporaryPpmOutput[4] = map(temporaryPpmOutput[4], 0, 0xff, 1000, 2000); - temporaryPpmOutput[5] = map(temporaryPpmOutput[5], 0, 0xff, 1000, 2000); - - //4bit channels - temporaryPpmOutput[6] = map(temporaryPpmOutput[6], 0, 0x0f, 1000, 2000); - temporaryPpmOutput[7] = map(temporaryPpmOutput[7], 0, 0x0f, 1000, 2000); - temporaryPpmOutput[8] = map(temporaryPpmOutput[8], 0, 0x0f, 1000, 2000); - temporaryPpmOutput[9] = map(temporaryPpmOutput[9], 0, 0x0f, 1000, 2000); - - /* - * Copy tremporary to real output - */ - for (uint8_t i = 0; i < PPM_CHANNEL_COUNT; i++) { - ppm[i] = temporaryPpmOutput[i]; - } -} - -void qspDecodeIncomingFrame(uint8_t incomingByte) { +void qspDecodeIncomingFrame(uint8_t incomingByte) +{ static uint8_t frameId; static uint8_t payloadLength; static uint8_t receivedPayload; static uint8_t packetId; //TODO move this to global scope maybe? - if (protocolState == IDLE && incomingByte == QSP_PREAMBLE) { + if (protocolState == IDLE && incomingByte == QSP_PREAMBLE) + { //If in IDLE and correct preamble comes, start to decode frame protocolState = PREAMBLE_RECEIVED; qspCrc = 0 ^ incomingByte; - } else if (protocolState == PREAMBLE_RECEIVED) { + } + else if (protocolState == PREAMBLE_RECEIVED) + { // Check if incomming channel ID is the same as receiver - if (incomingByte == CHANNEL_ID) { + if (incomingByte == CHANNEL_ID) + { protocolState = CHANNEL_RECEIVED; qspCrc ^= incomingByte; - for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) { + for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) + { qspPayload[i] = 0x00; } receivedPayload = 0; packetId = 0; - } else { + } + else + { protocolState = IDLE; } - } else if (protocolState == CHANNEL_RECEIVED) { + } + else if (protocolState == CHANNEL_RECEIVED) + { //Frame ID and payload length qspCrc ^= incomingByte; @@ -128,12 +104,15 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) { payloadLength = incomingByte & 0x0f; protocolState = FRAME_TYPE_RECEIVED; - - } else if (protocolState == FRAME_TYPE_RECEIVED) { + } + else if (protocolState == FRAME_TYPE_RECEIVED) + { qspCrc ^= incomingByte; packetId = incomingByte; protocolState = PACKET_ID_RECEIVED; - } else if (protocolState == PACKET_ID_RECEIVED) { + } + else if (protocolState == PACKET_ID_RECEIVED) + { //Now it's time for payload qspCrc ^= incomingByte; @@ -141,32 +120,37 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) { receivedPayload++; - if (receivedPayload == payloadLength) { - protocolState = PAYLOAD_RECEIVED; + if (receivedPayload == payloadLength) + { + protocolState = PAYLOAD_RECEIVED; } + } + else if (protocolState == PAYLOAD_RECEIVED) + { - } else if (protocolState == PAYLOAD_RECEIVED) { + if (qspCrc == incomingByte) + { +//CRC is correct - if (qspCrc == incomingByte) { - //CRC is correct - - #ifdef DEVICE_MODE_RX +#ifdef DEVICE_MODE_RX //If devide received a valid frame, that means it can start to talk canTransmit = true; - #endif +#endif - switch (frameId) { - case QSP_FRAME_RC_DATA: - qspDecodeRcDataFrame(); - break; + switch (frameId) + { + case QSP_FRAME_RC_DATA: + qspDecodeRcDataFrame(qspPayload, ppm); + break; - default: - //Unknown frame - //TODO do something in this case - break; + default: + //Unknown frame + //TODO do something in this case + break; } - - } else { + } + else + { //CRC failed, frame has to be rejected //TODO do something in this case or something } @@ -174,13 +158,13 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) { // In both cases switch to listening for next preamble protocolState = IDLE; } - } -void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) { +void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t payload[]) +{ //Zero CRC qspCrc = 0; - + //Write preamble writeToRadio(QSP_PREAMBLE); //Write CHANNEL_ID @@ -195,7 +179,8 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) { writeToRadio(qspGetPacketId()); //Write payload - for (uint8_t i = 0; i < length; i++) { + for (uint8_t i = 0; i < length; i++) + { writeToRadio(payload[i]); } @@ -209,44 +194,36 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) { static uint32_t lastRcFrameTransmit = 0; -uint8_t get10bitHighShift(uint8_t channel) { - return ((channel % 4) * 2) + 2; -} - -uint8_t get10bitLowShift(uint8_t channel) { - return 8 - get10bitHighShift(channel); -} - -void computeCrc(uint8_t dataByte) { - qspCrc ^= dataByte; -} - /* * Serial port used to send data */ #ifdef LORA_HARDWARE_SERIAL -int getRadioRssi(void) { +int getRadioRssi(void) +{ return 0; } -int getRadioSnr(void) { +float getRadioSnr(void) +{ return 0; } -void radioPacketStart(void) { - +void radioPacketStart(void) +{ } -void radioPacketEnd(void) { +void radioPacketEnd(void) +{ Serial.end(); delay(E45_TTL_100_UART_DOWNTIME); Serial.begin(UART_SPEED); } -void writeToRadio(uint8_t dataByte) { +void writeToRadio(uint8_t dataByte) +{ //Compute CRC - computeCrc(dataByte); + qspComputeCrc(&qspCrc, dataByte); //Write to radio Serial.write(dataByte); @@ -256,25 +233,30 @@ void writeToRadio(uint8_t dataByte) { #ifdef LORA_HARDWARE_SPI -int getRadioRssi(void) { +int getRadioRssi(void) +{ return LoRa.packetRssi(); } -int getRadioSnr(void) { +float getRadioSnr(void) +{ return LoRa.packetSnr(); } -void radioPacketStart(void) { +void radioPacketStart(void) +{ LoRa.beginPacket(); } -void radioPacketEnd(void) { +void radioPacketEnd(void) +{ LoRa.endPacket(); } -void writeToRadio(uint8_t dataByte) { +void writeToRadio(uint8_t dataByte) +{ //Compute CRC - computeCrc(dataByte); + qspComputeCrc(&qspCrc, dataByte); //Write to radio LoRa.write(dataByte); @@ -290,16 +272,19 @@ display.print(remoteData.latitude); display.display(); */ -void setup(void) { +void setup(void) +{ #ifdef LORA_HARDWARE_SERIAL Serial.begin(UART_SPEED); #endif #ifdef LORA_HARDWARE_SPI - if (!LoRa.begin(868E6)) { + if (!LoRa.begin(868E6)) + { Serial.println("LoRa init failed. Check your connections."); - while (true); + while (true) + ; } LoRa.onReceive(onReceive); LoRa.receive(); @@ -311,110 +296,76 @@ void setup(void) { /* * Initialize OLED display */ - display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) + display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) display.setTextSize(1); display.setTextColor(WHITE); display.clearDisplay(); display.display(); //initiallize default ppm values - for(int i=0; iget(i), 1000, 2000, 0, 1000) & 0x03ff; - uint8_t channelValue8 = map(ppmSource->get(i), 1000, 2000, 0, 255) & 0xff; - uint8_t channelValue4 = map(ppmSource->get(i), 1000, 2000, 0, 15) & 0x0f; +ISR(TIMER1_COMPA_vect) +{ //leave this alone + static boolean state = true; - if (i < 4) { - /* - * First 4 channels encoded with 10 bits - */ - uint8_t bitIndex = i + (i / 4); - qspPayload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i)); - qspPayload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i)); - } else if (i == 4 || i == 5) { - /* - * Next 2 with 8 bits - */ - qspPayload[i + 1] |= channelValue8; - } else if (i == 6) { - /* - * And last 4 with 4 bits per channel - */ - qspPayload[7] |= (channelValue4 << 4) & B11110000; - } else if (i == 7) { - qspPayload[7] |= channelValue4 & B00001111; - } else if (i == 8) { - qspPayload[8] |= (channelValue4 << 4) & B11110000; - } else if (i == 9) { - qspPayload[8] |= channelValue4 & B00001111; + TCNT1 = 0; + + if (state) + { //start pulse + digitalWrite(PPM_OUTPUT_PIN, PPM_SIGNAL_POSITIVE_STATE); + OCR1A = PPM_PULSE_LENGTH * 2; + state = false; + } + else + { //end pulse and calculate when to start the next pulse + static byte cur_chan_numb; + static unsigned int calc_rest; + + digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); + state = true; + + if (cur_chan_numb >= PPM_CHANNEL_COUNT) + { + cur_chan_numb = 0; + calc_rest = calc_rest + PPM_PULSE_LENGTH; // + OCR1A = (PPM_FRAME_LENGTH - calc_rest) * 2; + calc_rest = 0; + } + else + { + OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * 2; + calc_rest = calc_rest + ppm[cur_chan_numb]; + cur_chan_numb++; } } - - qspPayloadLength = 9; } #endif -#ifdef DEVICE_MODE_RX - -ISR(TIMER1_COMPA_vect){ //leave this alone - static boolean state = true; - - TCNT1 = 0; - - if (state) { //start pulse - digitalWrite(PPM_OUTPUT_PIN, PPM_SIGNAL_POSITIVE_STATE); - OCR1A = PPM_PULSE_LENGTH * 2; - state = false; - } else{ //end pulse and calculate when to start the next pulse - static byte cur_chan_numb; - static unsigned int calc_rest; - - digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); - state = true; - - if(cur_chan_numb >= PPM_CHANNEL_COUNT){ - cur_chan_numb = 0; - calc_rest = calc_rest + PPM_PULSE_LENGTH;// - OCR1A = (PPM_FRAME_LENGTH - calc_rest) * 2; - calc_rest = 0; - } - else{ - OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * 2; - calc_rest = calc_rest + ppm[cur_chan_numb]; - cur_chan_numb++; - } - } - } - -#endif - -void loop(void) { +void loop(void) +{ bool transmitPayload = false; @@ -422,15 +373,16 @@ void loop(void) { uint32_t currentMillis = millis(); - //TODO It should be only possible to transmit when radio is not receiveing + //TODO It should be only possible to transmit when radio is not receiveing /* * RC_DATA QSP frame */ - if (currentMillis - lastRcFrameTransmit > TX_RC_FRAME_RATE && !transmitPayload && protocolState == IDLE) { - lastRcFrameTransmit = currentMillis; + if (currentMillis - lastRcFrameTransmit > TX_RC_FRAME_RATE && !transmitPayload && protocolState == IDLE) + { + lastRcFrameTransmit = currentMillis; qspClearPayload(); - encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT); + encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT, qspPayload, &qspPayloadLength); qspFrameToSend = QSP_FRAME_RC_DATA; transmitPayload = true; @@ -438,27 +390,31 @@ void loop(void) { #endif -#ifdef LORA_HARDWARE_SERIAL - if (Serial.available()) { +#ifdef LORA_HARDWARE_SERIAL + if (Serial.available()) + { qspDecodeIncomingFrame(Serial.read()); } #endif - if (canTransmit && transmitPayload) { + if (canTransmit && transmitPayload) + { transmitPayload = false; radioPacketStart(); qspEncodeFrame(qspFrameToSend, qspPayloadLength, qspPayload); radioPacketEnd(); } - } #ifdef LORA_HARDWARE_SPI -void onReceive(int packetSize) { - if (packetSize == 0) return; +void onReceive(int packetSize) +{ + if (packetSize == 0) + return; - while (LoRa.available()) { + while (LoRa.available()) + { qspDecodeIncomingFrame(LoRa.read()); } } diff --git a/qsp.cpp b/qsp.cpp new file mode 100644 index 0000000..859773f --- /dev/null +++ b/qsp.cpp @@ -0,0 +1,106 @@ +#include "Arduino.h" +#include "variables.h" +#include + +void qspDecodeRcDataFrame(uint8_t payload[], int output[]) { + int temporaryPpmOutput[PPM_CHANNEL_COUNT] = {0}; + //TODO fix it, baby :) + + temporaryPpmOutput[0] = (uint16_t) (((uint16_t) payload[0] << 2) & 0x3fc) | ((payload[1] >> 6) & 0x03); + temporaryPpmOutput[1] = (uint16_t) (((uint16_t) payload[1] << 4) & 0x3f0) | ((payload[2] >> 4) & 0x0F); + temporaryPpmOutput[2] = (uint16_t) (((uint16_t) payload[2] << 6) & 0x3c0) | ((payload[3] >> 2) & 0x3F); + temporaryPpmOutput[3] = (uint16_t) (((uint16_t) payload[3] << 8) & 0x300) | ((payload[4] >> 2) & 0xFF); + temporaryPpmOutput[4] = payload[5]; + temporaryPpmOutput[5] = payload[6]; + temporaryPpmOutput[6] = (payload[7] >> 4) & 0b00001111; + temporaryPpmOutput[7] = payload[7] & 0b00001111; + temporaryPpmOutput[8] = (payload[8] >> 4) & 0b00001111; + temporaryPpmOutput[9] = payload[8] & 0b00001111; + + //10bit channels + temporaryPpmOutput[0] = map(temporaryPpmOutput[0], 0, 1000, 1000, 2000); + temporaryPpmOutput[1] = map(temporaryPpmOutput[1], 0, 1000, 1000, 2000); + temporaryPpmOutput[2] = map(temporaryPpmOutput[2], 0, 1000, 1000, 2000); + temporaryPpmOutput[3] = map(temporaryPpmOutput[3], 0, 1000, 1000, 2000); + + //8bit channels + temporaryPpmOutput[4] = map(temporaryPpmOutput[4], 0, 0xff, 1000, 2000); + temporaryPpmOutput[5] = map(temporaryPpmOutput[5], 0, 0xff, 1000, 2000); + + //4bit channels + temporaryPpmOutput[6] = map(temporaryPpmOutput[6], 0, 0x0f, 1000, 2000); + temporaryPpmOutput[7] = map(temporaryPpmOutput[7], 0, 0x0f, 1000, 2000); + temporaryPpmOutput[8] = map(temporaryPpmOutput[8], 0, 0x0f, 1000, 2000); + temporaryPpmOutput[9] = map(temporaryPpmOutput[9], 0, 0x0f, 1000, 2000); + + /* + * Copy tremporary to real output + */ + for (uint8_t i = 0; i < PPM_CHANNEL_COUNT; i++) { + output[i] = temporaryPpmOutput[i]; + } +} + +uint8_t get10bitHighShift(uint8_t channel) { + return ((channel % 4) * 2) + 2; +} + +uint8_t get10bitLowShift(uint8_t channel) { + return 8 - get10bitHighShift(channel); +} + +void qspComputeCrc(uint8_t *crc, uint8_t dataByte) +{ + *crc ^= dataByte; +} + +/** + * Encode 10 RC channels + */ +void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t payload[], uint8_t *payloadLength) +{ + for (uint8_t i = 0; i < noOfChannels; i++) + { + uint16_t channelValue10 = map(ppmSource->get(i), 1000, 2000, 0, 1000) & 0x03ff; + uint8_t channelValue8 = map(ppmSource->get(i), 1000, 2000, 0, 255) & 0xff; + uint8_t channelValue4 = map(ppmSource->get(i), 1000, 2000, 0, 15) & 0x0f; + + if (i < 4) + { + /* + * First 4 channels encoded with 10 bits + */ + uint8_t bitIndex = i + (i / 4); + payload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i)); + payload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i)); + } + else if (i == 4 || i == 5) + { + /* + * Next 2 with 8 bits + */ + payload[i + 1] |= channelValue8; + } + else if (i == 6) + { + /* + * And last 4 with 4 bits per channel + */ + payload[7] |= (channelValue4 << 4) & B11110000; + } + else if (i == 7) + { + payload[7] |= channelValue4 & B00001111; + } + else if (i == 8) + { + payload[8] |= (channelValue4 << 4) & B11110000; + } + else if (i == 9) + { + payload[8] |= channelValue4 & B00001111; + } + } + + *payloadLength = 9; +} \ No newline at end of file diff --git a/qsp.h b/qsp.h new file mode 100644 index 0000000..fc77883 --- /dev/null +++ b/qsp.h @@ -0,0 +1,8 @@ +#include "Arduino.h" +#include + +void qspDecodeRcDataFrame(uint8_t payload[], int output[]); +uint8_t get10bitHighShift(uint8_t channel); +uint8_t get10bitLowShift(uint8_t channel); +void qspComputeCrc(uint8_t *crc, uint8_t dataByte); +void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t payload[], uint8_t *payloadLength); \ No newline at end of file