Merge branch 'master' of github.com:DzikuVx/QuadMeUp_Crossbow

.vscode/arduino.json

@@ -1,6 +1,6 @@
 {
     "board": "bsfrance:avr:lora32u4",
     "sketch": "crossbow.ino",
-    "port": "COM17",
+    "port": "COM15",
     "output": "./build"
 }

README.md

@@ -13,9 +13,8 @@ Development, not yet functional
 | 1                     | Preamble | "Q" 0x51 |
 | 2                     | Channel ID | channel used for comunication between TX and RX |
 | 3                     | Frame type & Length | bits 7-5 defines frame, bits 4-0 payload length |
-| 4                     | Packet ID | |
 | 5 - 36                | Payload | 32 bytes max |
-| payload length + 5    | CRC | XOR of all previous bytes |
+| payload length + 4    | CRC | XOR of all previous bytes |
 
 ## Frame types
 
@@ -26,8 +25,8 @@ Development, not yet functional
 | 0010   | 0x2          | Request receiver configuration | TX -> RX |
 | 0011   | 0x3          | Receiver configuration | RX -> TX |
 | 0100   | 0x4          | Set receiver configuration | TX -> RX |
-| 0101   | 0x5          | PING frame | TX -> RX |
+| 0101   | 0x5          | PING frame, uses 9 byte payload | TX -> RX |
-| 0110   | 0x6          | PONG frame | RX -> TX |
+| 0110   | 0x6          | PONG frame, the same payload as PING | RX -> TX |
 
 ### `RC_DATA` frame format
 
@@ -48,7 +47,14 @@ Total length of `RC_DATA` payload is 9 bytes
 | 3     | RX supply volatage, sent in 0,1V      |
 | 4     | RX analog input 1 sent in 0,1V        |
 | 5     | RX analog input 2 sent in 0,1V        |
-| 6     | Last received packet ID               |
+| 6     | Flags                                 |
+
+#### Flags
+
+| Bit   | Meaning                               |
+| ----  | ----                                  |
+| 00000001  | Device in Failsafe mode           |
+
 
 ### `PING` and `PONG` frames
 

crossbow.ino

@@ -1,38 +1,43 @@
-#define LORA_HARDWARE_SPI
-
 // #define DEVICE_MODE_TX
 #define DEVICE_MODE_RX
 
+#define FEATURE_TX_OLED
+
 // #define DEBUG_SERIAL
 // #define DEBUG_PING_PONG
 // #define DEBUG_LED
 // #define WAIT_FOR_SERIAL
 
 #include <LoRa.h>
-// #include <PinChangeInterrupt.h>
 #include "variables.h"
-#include "sbus.h"
 #include "qsp.h"
 
+volatile int ppm[16] = {0};
+
 // LoRa32u4 ports
 #define LORA32U4_SS_PIN     8
 #define LORA32U4_RST_PIN    4
 #define LORA32U4_DI0_PIN    7
 
-int ppm[16] = {0};
-
 /*
  * Main defines for device working in TX mode
  */
 #ifdef DEVICE_MODE_TX
-
-// #define OLED_RESET -1
 #include <PPMReader.h>
-// #include <Adafruit_SSD1306.h>
-
 PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
-// PPMReader ppmReader(11, 2, MODE_PIN_CHANGE_INTERRUPT);
+
-// Adafruit_SSD1306 display(OLED_RESET);
+#include "txbuzzer.h"
+
+BuzzerState_t buzzer;
+
+#ifdef FEATURE_TX_OLED
+
+#define OLED_RESET -1
+#include <Adafruit_SSD1306.h>
+Adafruit_SSD1306 display(OLED_RESET);
+uint32_t lastOledTaskTime = 0;
+
+#endif
 
 #endif
 
@@ -40,6 +45,9 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
  * Main defines for device working in RX mode
  */
 #ifdef DEVICE_MODE_RX
+
+    #include "sbus.h"
+
     uint32_t sbusTime = 0;
     uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0};
     uint32_t lastRxStateTaskTime = 0;
@@ -48,10 +56,10 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
 /*
  * Start of QSP protocol implementation
  */
-QspConfiguration_t qsp = {};
+volatile QspConfiguration_t qsp = {};
-RxDeviceState_t rxDeviceState = {};
+volatile RxDeviceState_t rxDeviceState = {};
+volatile TxDeviceState_t txDeviceState = {};
 
-#ifdef LORA_HARDWARE_SPI
 
 uint8_t getRadioRssi(void)
 {
@@ -59,9 +67,9 @@ uint8_t getRadioRssi(void)
     return map(constrain(LoRa.packetRssi() * -1, 0, 164), 0, 164, 255, 0);
 }
 
-float getRadioSnr(void)
+uint8_t getRadioSnr(void)
 {
-    return (uint8_t) constrain(LoRa.packetSnr() * 10, 0, 255);
+    return (uint8_t) constrain(LoRa.packetSnr(), 0, 255);
 }
 
 void radioPacketStart(void)
@@ -85,16 +93,6 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
     LoRa.write(dataByte);
 }
 
-#endif
-
-/*
-display.clearDisplay();
-display.setCursor(0,0);
-display.print("Lat:");
-display.print(remoteData.latitude);
-display.display();
-*/
-
 void setup(void)
 {
 #ifdef DEBUG_SERIAL
@@ -109,8 +107,6 @@ void setup(void)
     qsp.deviceState = DEVICE_STATE_OK;
 #endif
 
-#ifdef LORA_HARDWARE_SPI
-
 #ifdef WAIT_FOR_SERIAL
     while (!Serial) {
         ; // wait for serial port to connect. Needed for native USB
@@ -134,24 +130,14 @@ void setup(void)
         while (true);
     }
 
-    LoRa.setSignalBandwidth(250E3);
+    LoRa.setSignalBandwidth(500E3);
-    LoRa.setSpreadingFactor(7);
+    LoRa.setSpreadingFactor(8);
-    LoRa.setCodingRate4(5);
+    LoRa.setCodingRate4(6);
-
+    LoRa.enableCrc();
     LoRa.onReceive(onReceive);
     LoRa.receive();
-#endif
 
 #ifdef DEVICE_MODE_RX
-    /*
-     * Initialize OLED display
-     */
-    // 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; i < 16; i++)
     {
@@ -169,17 +155,28 @@ void setup(void)
     TCCR1A = 0;  //reset timer1
     TCCR1B = 0;
     TCCR1B |= (1 << CS11);  //set timer1 to increment every 0,5 us or 1us on 8MHz
+
+#ifdef FEATURE_TX_OLED
+    display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
+    display.setTextSize(1);
+    display.setTextColor(WHITE);
+    display.clearDisplay();
+    display.display();
+#endif
+
+    /*
+     * TX should start talking imediately after power up
+     */
+    qsp.canTransmit = true;
+
+    pinMode(TX_BUZZER_PIN, OUTPUT);
+
+    //Play single tune to indicate power up
+    buzzerSingleMode(BUZZER_MODE_CHIRP, &buzzer);
 #endif
 
     pinMode(LED_BUILTIN, OUTPUT);
 
-/*
- * TX should start talking imediately after power up
- */
-#ifdef DEVICE_MODE_TX
-    qsp.canTransmit = true;
-#endif
-
 #ifdef DEBUG_SERIAL
     qsp.debugConfig |= DEBUG_FLAG_SERIAL;
 #endif
@@ -189,121 +186,294 @@ void setup(void)
 
 }
 
+int8_t txSendSequence[16] = {
+    QSP_FRAME_PING,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA,
+    QSP_FRAME_RC_DATA
+};
+
+int8_t rxSendSequence[16] = {
+    QSP_FRAME_RX_HEALTH,
+    -1,
+    -1,
+    -1,
+    QSP_FRAME_RX_HEALTH,
+    -1,
+    -1,
+    -1,
+    QSP_FRAME_RX_HEALTH,
+    -1,
+    -1,
+    -1,
+    QSP_FRAME_RX_HEALTH,
+    -1,
+    -1,
+    -1
+};
+
+uint8_t currentSequenceIndex = 0;
+#define TRANSMIT_SEQUENCE_COUNT 16
+
 #ifdef DEVICE_MODE_RX
 
 void updateRxDeviceState(RxDeviceState_t *rxDeviceState) {
     rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255);
     rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255);
     rxDeviceState->a2Voltage = map(analogRead(RX_ADC_PIN_3), 0, 1024, 0, 255);
-    rxDeviceState->rssi = getRadioRssi();
-    rxDeviceState->snr = getRadioSnr();
 }    
 
+int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
+    
+    if (qsp->forcePongFrame) {
+        qsp->forcePongFrame = false;
+        return QSP_FRAME_PONG;
+    }
+
+    int8_t retVal = rxSendSequence[currentSequenceIndex];
+    
+    currentSequenceIndex++;
+    if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) {
+        currentSequenceIndex = 0;
+    }
+
+    return retVal;
+}
+
+#endif
+
+#ifdef DEVICE_MODE_TX
+int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
+    int8_t retVal = txSendSequence[currentSequenceIndex];
+
+    currentSequenceIndex++;
+    if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) {
+        currentSequenceIndex = 0;
+    }
+
+    return retVal;
+}
 #endif
 
 void loop(void)
 {
+
+#ifdef DEVICE_MODE_TX
+    if (txDeviceState.readPacket) {
+        int incomingByte = LoRa.read();
+        if (incomingByte > -1) {
+            qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState);
+        } else {
+            txDeviceState.rssi = getRadioRssi();
+            txDeviceState.snr = getRadioSnr();
+            txDeviceState.readPacket = false;
+        }
+    }
+#endif
+
     uint32_t currentMillis = millis();
     bool transmitPayload = false;
+    static uint32_t previousAnyFrameReceivedAt = 0;
 
     /*
      * Watchdog for frame decoding stuck somewhere in the middle of a process
      */
     if (
         qsp.protocolState != QSP_STATE_IDLE && 
-        abs(currentMillis - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME 
+        qsp.frameDecodingStartedAt + QSP_MAX_FRAME_DECODE_TIME < currentMillis 
     ) {
         qsp.protocolState = QSP_STATE_IDLE;
     }
 
-    if (
-        qsp.forcePongFrame && 
-        !transmitPayload && 
-        qsp.protocolState == QSP_STATE_IDLE
-    )
-    {
-        qsp.forcePongFrame = false;
-        qsp.lastFrameTransmitedAt[QSP_FRAME_PONG] = currentMillis;
-        qsp.frameToSend = QSP_FRAME_PONG;
-        transmitPayload = true;
-    }
-
-
 #ifdef DEVICE_MODE_TX
 
-#ifdef DEBUG_PING_PONG
-    //PING frame
     if (
-        currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_PING] > TX_PING_RATE && 
+        qsp.protocolState == QSP_STATE_IDLE &&
-        !transmitPayload && 
+        qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
-        qsp.protocolState == QSP_STATE_IDLE
+    ) {
-    )
-    {
-        qsp.lastFrameTransmitedAt[QSP_FRAME_PING] = currentMillis;
-
-        qspClearPayload(&qsp);
-        encodePingPayload(&qsp, micros());
-        qsp.frameToSend = QSP_FRAME_PING;
         
-        transmitPayload = true;
+        int8_t frameToSend = getFrameToTransmit(&qsp);
-    }
-#endif
 
-    /*
+        if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
-     * RC_DATA QSP frame
+            frameToSend = -1;
-     */
+            //FIXME uncomment to enable full Failsafe
-    if (
+        }
-        currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] > TX_RC_FRAME_RATE && 
-        !transmitPayload && 
-        qsp.protocolState == QSP_STATE_IDLE
-    )
-    {
-        qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] = currentMillis;
 
-        qspClearPayload(&qsp);
+        if (frameToSend > -1) {
-        encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
-        qsp.frameToSend = QSP_FRAME_RC_DATA;
 
-        transmitPayload = true;
+            qsp.frameToSend = frameToSend;
+            qspClearPayload(&qsp);
+
+            switch (qsp.frameToSend) {
+                case QSP_FRAME_PING:
+                    encodePingPayload(&qsp, micros());
+                    break;
+
+                case QSP_FRAME_RC_DATA:
+                    encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
+                    break;
+            }
+
+            transmitPayload = true;
+        }
+
+        qsp.lastTxSlotTimestamp = currentMillis;
     }
 
 #endif
 
 #ifdef DEVICE_MODE_RX
 
-    if (currentMillis > sbusTime) {
-        sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
-        Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
-
-        sbusTime = currentMillis + SBUS_UPDATE_RATE;
-    }
-
     /*
      * This routine updates RX device state and updates one of radio channels with RSSI value
      */
-    if (currentMillis - lastRxStateTaskTime > RX_TASK_HEALTH) {
+    if (lastRxStateTaskTime + RX_TASK_HEALTH < currentMillis) {
         lastRxStateTaskTime = currentMillis;
         updateRxDeviceState(&rxDeviceState);
         ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 255, 1000, 2000);
+        if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
+            digitalWrite(LED_BUILTIN, HIGH);
+        } else {
+            digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
+        }
     }
-
+    
     /*
-     * RX_HEALTH QSP frame
+     * Main routine to answer to TX module
      */
-    if (
+    if (qsp.transmitWindowOpen && qsp.protocolState == QSP_STATE_IDLE) {
-        currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] > RX_RX_HEALTH_FRAME_RATE && 
+        qsp.transmitWindowOpen = false;
-        !transmitPayload && 
+
-        qsp.protocolState == QSP_STATE_IDLE
+        int8_t frameToSend = getFrameToTransmit(&qsp);
-    )
+        if (frameToSend > -1) {
-    {
+            qsp.frameToSend = frameToSend;
-        qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] = currentMillis;
+
-        qspClearPayload(&qsp);
+            if (frameToSend != QSP_FRAME_PONG) {
-        encodeRxHealthPayload(&qsp, &rxDeviceState);
+                qspClearPayload(&qsp);
-        qsp.frameToSend = QSP_FRAME_RX_HEALTH;
+            }
+            switch (qsp.frameToSend) {
+                case QSP_FRAME_PONG:
+                    /*
+                     * Pong frame just responses with received payload
+                     */
+                    break;
+
+                case QSP_FRAME_RX_HEALTH:
+                    encodeRxHealthPayload(&qsp, &rxDeviceState);
+                    break;
+            }
+
+            transmitPayload = true;
+        }
 
-        transmitPayload = true;
     }
 
+    if (currentMillis > sbusTime) {
+        sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
+        Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
+        sbusTime = currentMillis + SBUS_UPDATE_RATE;
+    }
+
+    if (qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA] + RX_FAILSAFE_DELAY < currentMillis) {
+        qsp.deviceState = DEVICE_STATE_FAILSAFE;
+    } else {
+        qsp.deviceState = DEVICE_STATE_OK;
+    }
+
+#endif
+
+#ifdef DEVICE_MODE_TX
+
+    buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer);
+
+    // This routing enables when TX starts to receive signal from RX for a first time or after 
+    // failsafe
+    if (txDeviceState.isReceiving == false && qsp.anyFrameRecivedAt != 0) {
+        //TX module started to receive data
+        buzzerSingleMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer);
+        txDeviceState.isReceiving = true;
+        qsp.deviceState = DEVICE_STATE_OK;
+    }
+
+    //Here we detect failsafe state on TX module
+    if (
+        txDeviceState.isReceiving && 
+        qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis
+    ) {
+        txDeviceState.isReceiving = false;
+        rxDeviceState.a1Voltage = 0;
+        rxDeviceState.a2Voltage = 0;
+        rxDeviceState.rxVoltage = 0;
+        rxDeviceState.rssi = 0;
+        rxDeviceState.snr = 0;
+        rxDeviceState.flags = 0;
+        txDeviceState.roundtrip = 0;
+        qsp.deviceState = DEVICE_STATE_FAILSAFE;
+        qsp.anyFrameRecivedAt = 0;
+    }
+
+    //FIXME rxDeviceState should be resetted also in RC_HEALT frame is not received in a long period 
+
+    //Handle audible alarms
+    if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
+        //Failsafe detected by TX
+        buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
+    } else if (txDeviceState.isReceiving && (rxDeviceState.flags & 0x1) == 1) {
+        //Failsafe reported by RX module
+        buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
+    } else if (txDeviceState.isReceiving && txDeviceState.rssi < 100) {
+        buzzerContinousMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer);
+    } else if (txDeviceState.isReceiving && txDeviceState.rssi < 128) {
+        buzzerContinousMode(BUZZER_MODE_CHIRP, &buzzer);
+    } else {
+        buzzerContinousMode(BUZZER_MODE_OFF, &buzzer);
+    }
+
+#ifdef FEATURE_TX_OLED
+    if (
+        currentMillis - lastOledTaskTime > OLED_UPDATE_RATE
+    ) {
+        lastOledTaskTime = currentMillis;
+
+        display.clearDisplay();
+
+        display.setTextColor(WHITE, BLACK);
+        display.setCursor(0, 0);
+        display.setTextSize(3);
+        display.print(map(txDeviceState.rssi, 0, 255, 0, 164)); 
+
+        display.setCursor(18, 28);
+        display.setTextSize(2);
+        display.print(txDeviceState.snr);
+        
+        display.setCursor(74, 0);
+        display.setTextSize(3);
+        display.print(map(rxDeviceState.rssi, 0, 255, 0, 164)); 
+
+        display.setCursor(92, 28);
+        display.setTextSize(2);
+        display.print(rxDeviceState.snr); 
+
+        display.setCursor(54, 48);
+        display.setTextSize(2);
+        display.print(txDeviceState.roundtrip); 
+
+        display.display(); 
+    }
+#endif
+
 #endif
 
     if (qsp.canTransmit && transmitPayload)
@@ -311,40 +481,30 @@ void loop(void)
         radioPacketStart();
         qspEncodeFrame(&qsp);
         radioPacketEnd();
-
-    #ifdef DEBUG_LED
-        digitalWrite(LED_BUILTIN, HIGH);
-        delay(10);
-        digitalWrite(LED_BUILTIN, LOW);
-        delay(70);
-        digitalWrite(LED_BUILTIN, HIGH);
-        delay(10);
-        digitalWrite(LED_BUILTIN, LOW);
-    #endif
         transmitPayload = false;
     }
 
-    /*
-     * Here we do state handling and similar operations 
-     */
-#ifdef DEVICE_MODE_RX
-    if (abs(currentMillis - qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA]) > RX_FAILSAFE_DELAY) {
-        qsp.deviceState = DEVICE_STATE_FAILSAFE;
-    } else {
-        qsp.deviceState = DEVICE_STATE_OK;
-    }
-#endif
 }
 
-#ifdef LORA_HARDWARE_SPI
 void onReceive(int packetSize)
 {
     if (packetSize == 0)
         return;
 
-    while (LoRa.available())
+#ifdef DEVICE_MODE_TX
+    txDeviceState.readPacket = true;
+#endif
+
+#ifdef DEVICE_MODE_RX
+
+    int incomingByte;
+
+    while (incomingByte = LoRa.read(), incomingByte > -1)
     {
-        qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState);
+        qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState);
     }
-}
+
-#endif
+    rxDeviceState.rssi = getRadioRssi();
+    rxDeviceState.snr = getRadioSnr();
+#endif
+}

qsp.cpp

@@ -60,7 +60,14 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
     qsp->payload[2] = rxDeviceState->rxVoltage;
     qsp->payload[3] = rxDeviceState->a1Voltage;
     qsp->payload[4] = rxDeviceState->a2Voltage;
-    qsp->payload[5] = qsp->lastReceivedPacketId;
+
+    uint8_t flags = 0;
+
+    if (qsp->deviceState == DEVICE_STATE_FAILSAFE) {
+        flags |= 0x01 << 0;
+    }
+
+    qsp->payload[5] = flags;
 
     qsp->payloadLength = 6;
 }
@@ -71,7 +78,7 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
     rxDeviceState->rxVoltage = qsp->payload[2];
     rxDeviceState->a1Voltage = qsp->payload[3];
     rxDeviceState->a2Voltage = qsp->payload[4];
-    // rxDeviceState->rssi = qsp->payload[0]; //TODO we skipped decoding this byte, figure it out
+    rxDeviceState->flags = qsp->payload[5];
 }
 
 /**
@@ -127,13 +134,6 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t
     qsp->payloadLength = 9;
 }
 
-uint8_t qspGetPacketId()
-{
-    static uint8_t packetId = 0;
-
-    return packetId++;
-}
-
 void qspClearPayload(QspConfiguration_t *qsp)
 {
     for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
@@ -143,16 +143,19 @@ void qspClearPayload(QspConfiguration_t *qsp)
     qsp->payloadLength = 0;
 }
 
-void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[], RxDeviceState_t *rxDeviceState)
+void qspDecodeIncomingFrame(
-{
+    QspConfiguration_t *qsp, 
+    uint8_t incomingByte, 
+    int ppm[], 
+    RxDeviceState_t *rxDeviceState, 
+    TxDeviceState_t *txDeviceState
+) {
     static uint8_t frameId;
     static uint8_t payloadLength;
     static uint8_t receivedPayload;
-    static uint8_t packetId; //TODO move this to global scope maybe?
 
     if (qsp->protocolState == QSP_STATE_IDLE && incomingByte == QSP_PREAMBLE)
     {
-        //FIXME there should be a way to go back to IDLE if frame did not finished decoding in reasonable time
         //If in IDLE and correct preamble comes, start to decode frame
         qsp->protocolState = QSP_STATE_PREAMBLE_RECEIVED;
         qsp->crc = 0 ^ incomingByte;
@@ -169,7 +172,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
             qspClearPayload(qsp);
 
             receivedPayload = 0;
-            packetId = 0;
         }
         else
         {
@@ -188,12 +190,9 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
     }
     else if (qsp->protocolState == QSP_STATE_FRAME_TYPE_RECEIVED)
     {
-        qsp->crc ^= incomingByte;
+        if (receivedPayload >= QSP_PAYLOAD_LENGTH) {
-        packetId = incomingByte;
+            qsp->protocolState = QSP_STATE_IDLE;
-        qsp->protocolState = QSP_STATE_PACKET_ID_RECEIVED;
+        }
-    }
-    else if (qsp->protocolState == QSP_STATE_PACKET_ID_RECEIVED)
-    {
 
         //Now it's time for payload
         qsp->crc ^= incomingByte;
@@ -209,7 +208,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
     }
     else if (qsp->protocolState == QSP_STATE_PAYLOAD_RECEIVED)
     {
-
         if (qsp->crc == incomingByte) {
             //CRC is correct
 
@@ -220,22 +218,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
             if (frameId < QSP_FRAME_COUNT) {
                 qsp->lastFrameReceivedAt[frameId] = millis();
             }
+            qsp->anyFrameRecivedAt = millis();
 
-            qsp->lastReceivedPacketId = packetId;
-
-            if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
-                Serial.print("Frame ");
-                Serial.print(frameId);
-                Serial.println(" received");
-            }
-
-            if (qsp->debugConfig & DEBUG_FLAG_LED) {
-                digitalWrite(LED_BUILTIN, HIGH);
-                delay(10);
-                digitalWrite(LED_BUILTIN, LOW);
-                delay(100);
-            }
-            
             switch (frameId) {
                 case QSP_FRAME_RC_DATA:
                     qspDecodeRcDataFrame(qsp, ppm);
@@ -243,12 +227,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
 
                 case QSP_FRAME_RX_HEALTH:
                     decodeRxHealthPayload(qsp, rxDeviceState);
-                    if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
-                        Serial.print("RX RSSI: ");
-                        Serial.println(rxDeviceState->rssi);
-                        Serial.print("RX SNR: ");
-                        Serial.println(rxDeviceState->snr);
-                    }
                     break;
 
                 case QSP_FRAME_PING:
@@ -256,18 +234,13 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
                     break;
 
                 case QSP_FRAME_PONG:
-                    if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
+                    txDeviceState->roundtrip = qsp->payload[0];
+                    txDeviceState->roundtrip += (uint32_t) qsp->payload[1] << 8;
+                    txDeviceState->roundtrip += (uint32_t) qsp->payload[2] << 16;
+                    txDeviceState->roundtrip += (uint32_t) qsp->payload[3] << 24;
 
-                        uint32_t incoming = 0;
+                    txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
 
-                        incoming = qsp->payload[0];
-                        incoming += (uint32_t) qsp->payload[1] << 8;
-                        incoming += (uint32_t) qsp->payload[2] << 16;
-                        incoming += (uint32_t) qsp->payload[3] << 24;
-
-                        Serial.print("Rountrip: ");
-                        Serial.println((micros() - incoming) / 1000);
-                    }
                     break;
 
                 default:
@@ -275,6 +248,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
                     //TODO do something in this case
                     break;
             }
+
+            qsp->transmitWindowOpen = true;
         }
         else
         {
@@ -304,9 +279,6 @@ void qspEncodeFrame(QspConfiguration_t *qsp) {
     data |= (qsp->frameToSend << 4) & 0xf0;
     qsp->hardwareWriteFunction(data, qsp);
 
-    //Write packet ID
-    qsp->hardwareWriteFunction(qspGetPacketId(), qsp);
-
     //Write payload
     for (uint8_t i = 0; i < qsp->payloadLength; i++)
     {

qsp.h

@@ -9,8 +9,13 @@ uint8_t get10bitLowShift(uint8_t channel);
 void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
 void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
 void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels);
-uint8_t qspGetPacketId(void);
+void qspDecodeIncomingFrame(
-void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[], RxDeviceState_t *rxDeviceState);
+    QspConfiguration_t *qsp, 
+    uint8_t incomingByte, 
+    int ppm[], 
+    RxDeviceState_t *rxDeviceState,
+    TxDeviceState_t *txDeviceState
+);
 void qspClearPayload(QspConfiguration_t *qsp);
 void qspEncodeFrame(QspConfiguration_t *qsp);
 

sbus.cpp

@@ -12,6 +12,15 @@
 #define SBUS_STATE_FAILSAFE 0x08
 #define SBUS_STATE_SIGNALLOSS 0x04
 
+/*
+Precomputed mapping from 990-2010 to 173:1811
+equivalent to 
+map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
+*/
+int mapChannelToSbus(int in) {
+    return (((long) in * 1605l) / 1000l) - 1417;
+}
+
 void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
     
     static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -21,7 +30,7 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
      * 173-1811 with middle at 992 S.BUS protocol requires
      */
     for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
-        output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
+        output[i] = mapChannelToSbus(channels[i]);
     }
 
     uint8_t stateByte = 0x00;

track1.txt

@@ -0,0 +1,31 @@
+2
+3
+4
+5
+12
+13
+12
+13
+12
+13
+12
+13
+12
+13
+12
+13
+12
+13
+12
+13
+12
+13
+6
+7
+12
+13
+12
+13
+12
+13
+2

track2.txt

@@ -0,0 +1,14 @@
+13
+12
+13
+6
+7
+12
+13
+12
+13
+12
+13
+12
+13
+2

track3.txt

@@ -0,0 +1,10 @@
+12
+13
+12
+13
+2
+21
+22
+23
+24
+25

txbuzzer.cpp

@@ -0,0 +1,66 @@
+#include "Arduino.h"
+#include "txbuzzer.h"
+
+/**
+ * This method plays selected pattern only once
+ * It disables continious mode 
+ */
+void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer) {
+    buzzer->singleModeEnabled = true;
+    buzzer->enabled = false;
+    buzzer->mode = mode;
+    buzzer->tick = 0;
+}
+
+void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer) {
+    buzzer->singleModeEnabled = false;
+    buzzer->enabled = true;
+    buzzer->mode = mode;
+}
+
+void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer)
+{
+
+    if (!buzzer->enabled && !buzzer->singleModeEnabled)
+    {
+        digitalWrite(pin, LOW);
+        return;
+    }
+
+    if (timestamp > buzzer->updateTime)
+    {
+
+        int8_t currentPattern = buzzer->pattern[buzzer->mode][buzzer->element];
+
+        if (currentPattern == PATTERN_CYCLE_OFF)
+        {
+            digitalWrite(pin, LOW);
+        }
+        else if (currentPattern == PATTERN_CYCLE_ON)
+        {
+            digitalWrite(pin, HIGH);
+        }
+        else if (currentPattern == PATTERN_CYCLE_IGNORE || currentPattern == buzzer->tick)
+        {
+
+            if (currentPattern != PATTERN_CYCLE_IGNORE)
+            {
+                digitalWrite(pin, !digitalRead(pin));
+            }
+
+            buzzer->element++;
+            if (buzzer->element == PATTERN_ELEMENT_NUMBER)
+            {
+                buzzer->element = 0;
+            }
+        }
+
+        buzzer->tick++;
+        if (buzzer->tick >= buzzer->patternMaxTick)
+        {
+            buzzer->tick = 0;
+            buzzer->singleModeEnabled = false;
+        }
+        buzzer->updateTime = timestamp + buzzer->patternTickPerdiod;
+    }
+};

txbuzzer.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "Arduino.h"
+
+#define PATTERN_CYCLE_OFF 127
+#define PATTERN_CYCLE_ON -1
+#define PATTERN_CYCLE_IGNORE -2
+
+#define PATTERN_MODES_NUMBER 6
+#define PATTERN_ELEMENT_NUMBER 4
+
+enum {
+    BUZZER_MODE_OFF = 0,
+    BUZZER_MODE_CONTINUOUS = 1,
+    BUZZER_MODE_SLOW_BEEP = 2,
+    BUZZER_MODE_FAST_BEEP = 3,
+    BUZZER_MODE_CHIRP = 4,
+    BUZZER_MODE_DOUBLE_CHIRP = 5
+};
+
+struct BuzzerState_t {
+    bool enabled = false; //Continous mode buzzer
+    bool singleModeEnabled = false;
+    uint8_t mode = BUZZER_MODE_OFF;
+    
+    uint32_t updateTime = 0;
+
+    uint8_t tick = 0;
+    uint8_t element = 0;
+    const uint8_t patternMaxTick = 20;
+    const uint8_t patternTickPerdiod = 75;
+
+    const int8_t pattern[PATTERN_MODES_NUMBER][PATTERN_ELEMENT_NUMBER] = {
+        {PATTERN_CYCLE_OFF},
+        {PATTERN_CYCLE_ON},
+        {0, 7, 10, 17},
+        {0, 4, 10, 14},
+        {0, 1, PATTERN_CYCLE_IGNORE, PATTERN_CYCLE_IGNORE},
+        {0, 1, 2, 3}
+    };
+
+};
+
+void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer);
+void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer);
+void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer);

variables.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#define OLED_UPDATE_RATE 500
+
 #define SBUS_UPDATE_RATE 15 //ms
 #define SBUS_PACKET_LENGTH 25
 
@@ -9,17 +11,15 @@
 #define RX_TASK_HEALTH 200 //5Hz should be enough
 #define RSSI_CHANNEL 11
 
-#define RX_RX_HEALTH_FRAME_RATE 1000
+#define TX_TRANSMIT_SLOT_RATE 70 //ms
-#define TX_RC_FRAME_RATE 500 //ms
+#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8)
-#define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8)
+#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4)
-
-#define TX_PING_RATE 2000 
 
 #define CHANNEL_ID 0x01
 #define QSP_PREAMBLE 0x51
 #define QSP_PAYLOAD_LENGTH 32
 
-#define QSP_MAX_FRAME_DECODE_TIME 50 //max time that frame can be decoded in ms
+#define QSP_MAX_FRAME_DECODE_TIME 10 //max time that frame can be decoded in ms
 
 #define QSP_FRAME_RC_DATA 0x0
 #define QSP_FRAME_RX_HEALTH 0x1
@@ -39,14 +39,14 @@ enum dataStates {
     QSP_STATE_PREAMBLE_RECEIVED,
     QSP_STATE_CHANNEL_RECEIVED,
     QSP_STATE_FRAME_TYPE_RECEIVED,
-    QSP_STATE_PACKET_ID_RECEIVED,
     QSP_STATE_PAYLOAD_RECEIVED,
     QSP_STATE_CRC_RECEIVED
 };
 
 enum deviceStates {
     DEVICE_STATE_OK,
-    DEVICE_STATE_FAILSAFE
+    DEVICE_STATE_FAILSAFE,
+    DEVICE_STATE_UNDETERMINED
 };
 
 enum debugConfigFlags {
@@ -60,6 +60,8 @@ enum debugConfigFlags {
 #define PPM_INPUT_CHANNEL_COUNT 10
 #define PPM_OUTPUT_CHANNEL_COUNT 10
 
+#define TX_BUZZER_PIN A5
+
 #define PPM_CHANNEL_DEFAULT_VALUE 1500  //set the default servo value
 #define PPM_FRAME_LENGTH 30500  //set the PPM frame length in microseconds (1ms = 1000µs)
 #define PPM_PULSE_LENGTH 300  //set the pulse length
@@ -74,20 +76,31 @@ struct QspConfiguration_t {
     uint8_t payloadLength = 0;
     uint8_t frameToSend = 0;
     uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
-    uint32_t lastFrameTransmitedAt[QSP_FRAME_COUNT] = {0};
+    uint32_t anyFrameRecivedAt = 0;
-    uint8_t deviceState = DEVICE_STATE_OK;
+    uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
     void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
-    uint8_t lastReceivedPacketId = 0;
     bool canTransmit = false;
     bool forcePongFrame = false;
     uint8_t debugConfig = 0;
     uint32_t frameDecodingStartedAt = 0;
+    uint32_t lastTxSlotTimestamp = 0;
+    bool transmitWindowOpen = false;
+};
+
+struct TxDeviceState_t {
+    uint8_t rssi = 0;
+    uint8_t snr = 0;
+    uint8_t flags = 0;
+    uint32_t roundtrip = 0;
+    bool readPacket = false;
+    bool isReceiving = false; //Indicates that TX module is receiving frames from RX module
 };
 
 struct RxDeviceState_t {
-    int rssi = 0;
+    uint8_t rssi = 0;
-    float snr = 0;
+    uint8_t snr = 0;
     uint8_t rxVoltage = 0;
     uint8_t a1Voltage = 0;
     uint8_t a2Voltage = 0;
+    uint8_t flags = 0;
 };