Merge pull request #27 from DzikuVx/qsp-callbacks

Air protocol decoding using callbacks

crossbow.ino

@@ -13,8 +13,6 @@
 #include "main_variables.h"
 #include "qsp.h"
 
-int ppm[16] = {0};
-
 // LoRa32u4 ports
 #define LORA32U4_SS_PIN     8
 #define LORA32U4_RST_PIN    4
@@ -81,6 +79,50 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
     LoRa.write(dataByte);
 }
 
+void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, RadioState_t *radioState) {
+    //If devide received a valid frame, that means it can start to talk
+    qsp->canTransmit = true;
+
+    //Store the last timestamp when frame was received
+    if (qsp->frameId < QSP_FRAME_COUNT) {
+        qsp->lastFrameReceivedAt[qsp->frameId] = millis();
+    }
+    qsp->anyFrameRecivedAt = millis();
+    switch (qsp->frameId) {
+        case QSP_FRAME_RC_DATA:
+            qspDecodeRcDataFrame(qsp, rxDeviceState);
+            break;
+
+        case QSP_FRAME_RX_HEALTH:
+            decodeRxHealthPayload(qsp, rxDeviceState);
+            break;
+
+        case QSP_FRAME_PING:
+            qsp->forcePongFrame = true;
+            break;
+
+        case QSP_FRAME_PONG:
+            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;
+
+            txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
+            break;
+
+        default:
+            //Unknown frame
+            //TODO do something in this case
+            break;
+    }
+
+    qsp->transmitWindowOpen = true;
+}
+
+void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, RadioState_t *radioState) {
+
+}
+
 void setup(void)
 {
 #ifdef DEBUG_SERIAL
@@ -88,6 +130,8 @@ void setup(void)
 #endif
 
     qsp.hardwareWriteFunction = writeToRadio;
+    qsp.onSuccessCallback = onQspSuccess;
+    qsp.onFailureCallback = onQspFailure;
 
 #ifdef DEVICE_MODE_RX
     qsp.deviceState = DEVICE_STATE_FAILSAFE;
@@ -123,7 +167,7 @@ void setup(void)
     //initiallize default ppm values
     for (int i = 0; i < 16; i++)
     {
-        ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
+        rxDeviceState.channels[i] = PPM_CHANNEL_DEFAULT_VALUE;
     }
 
     pinMode(RX_ADC_PIN_1, INPUT);
@@ -217,7 +261,7 @@ void loop(void)
     if (radioState.bytesToRead != NO_DATA_TO_READ) {
 
         for (uint8_t i = 0; i < radioState.bytesToRead; i++) {
-            qspDecodeIncomingFrame(&qsp, radioState.data[i], ppm, &rxDeviceState, &txDeviceState);
+            qspDecodeIncomingFrame(&qsp, radioState.data[i], &rxDeviceState, &txDeviceState, &radioState);
         }
 
         radioState.bytesToRead = NO_DATA_TO_READ;
@@ -283,7 +327,7 @@ void loop(void)
     if (lastRxStateTaskTime + RX_TASK_HEALTH < currentMillis) {
         lastRxStateTaskTime = currentMillis;
         updateRxDeviceState(&rxDeviceState);
-        ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 164, 1000, 2000);
+        rxDeviceState.channels[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 164, 1000, 2000);
         if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
             digitalWrite(LED_BUILTIN, HIGH);
         } else {
@@ -322,7 +366,7 @@ void loop(void)
     }
 
     if (currentMillis > sbusTime) {
-        sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
+        sbusPreparePacket(sbusPacket, rxDeviceState.channels, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
         Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
         sbusTime = currentMillis + SBUS_UPDATE_RATE;
     }

qsp.cpp

@@ -2,7 +2,7 @@
 #include "variables.h"
 #include <PPMReader.h>
 
-void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]) {
+void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
     int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0};
     //TODO fix it, baby :)
 
@@ -37,7 +37,7 @@ void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]) {
      * Copy tremporary to real output
      */
     for (uint8_t i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++) {
-        output[i] = temporaryPpmOutput[i];
+        rxDeviceSate->channels[i] = temporaryPpmOutput[i];
     }
 }
 
@@ -159,9 +159,9 @@ void qspClearPayload(QspConfiguration_t *qsp)
 void qspDecodeIncomingFrame(
     QspConfiguration_t *qsp, 
     uint8_t incomingByte, 
-    int ppm[], 
     RxDeviceState_t *rxDeviceState, 
-    TxDeviceState_t *txDeviceState
+    TxDeviceState_t *txDeviceState,
+    RadioState_t *radioState
 ) {
     static uint8_t frameId;
     static uint8_t payloadLength;
@@ -190,7 +190,7 @@ void qspDecodeIncomingFrame(
         //Frame ID and payload length
         qspComputeCrc(qsp, incomingByte);
 
-        frameId = (incomingByte >> 4) & 0x0f;
+        qsp->frameId = (incomingByte >> 4) & 0x0f;
         payloadLength = incomingByte & 0x0f;
         
         qsp->protocolState = QSP_STATE_FRAME_TYPE_RECEIVED;
@@ -217,49 +217,9 @@ void qspDecodeIncomingFrame(
     {
         if (qsp->crc == incomingByte) {
             //CRC is correct
-
+            qsp->onSuccessCallback(qsp, txDeviceState, rxDeviceState, radioState);
-            //If devide received a valid frame, that means it can start to talk
+        } else {
-            qsp->canTransmit = true;
+            qsp->onFailureCallback(qsp, txDeviceState, rxDeviceState, radioState);
-
-            //Store the last timestamp when frame was received
-            if (frameId < QSP_FRAME_COUNT) {
-                qsp->lastFrameReceivedAt[frameId] = millis();
-            }
-            qsp->anyFrameRecivedAt = millis();
-            switch (frameId) {
-                case QSP_FRAME_RC_DATA:
-                    qspDecodeRcDataFrame(qsp, ppm);
-                    break;
-
-                case QSP_FRAME_RX_HEALTH:
-                    decodeRxHealthPayload(qsp, rxDeviceState);
-                    break;
-
-                case QSP_FRAME_PING:
-                    qsp->forcePongFrame = true;
-                    break;
-
-                case QSP_FRAME_PONG:
-                    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;
-
-                    txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
-                    break;
-
-                default:
-                    //Unknown frame
-                    //TODO do something in this case
-                    break;
-            }
-
-            qsp->transmitWindowOpen = true;
-        }
-        else
-        {
-            //CRC failed, frame has to be rejected
-            //TODO do something in this case or something
         }
 
         // In both cases switch to listening for next preamble

qsp.h

@@ -1,7 +1,7 @@
 #include "Arduino.h"
 #include <PPMReader.h>
 
-void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]);
+void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
 void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
 
 uint8_t get10bitHighShift(uint8_t channel);
@@ -12,9 +12,9 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t
 void qspDecodeIncomingFrame(
     QspConfiguration_t *qsp, 
     uint8_t incomingByte, 
-    int ppm[], 
     RxDeviceState_t *rxDeviceState,
-    TxDeviceState_t *txDeviceState
+    TxDeviceState_t *txDeviceState,
+    RadioState_t *radioState
 );
 void qspClearPayload(QspConfiguration_t *qsp);
 void qspEncodeFrame(QspConfiguration_t *qsp);

variables.h

@@ -85,24 +85,6 @@ struct RadioState_t {
     uint8_t data[20] = {0}; //Max size of packet that can be processed in QSP
 };
 
-struct QspConfiguration_t {
-    uint8_t protocolState = QSP_STATE_IDLE;
-    uint8_t crc = 0;
-    uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
-    uint8_t payloadLength = 0;
-    uint8_t frameToSend = 0;
-    uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
-    uint32_t anyFrameRecivedAt = 0;
-    uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
-    void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
-    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 flags = 0;
     uint32_t roundtrip = 0;
@@ -116,4 +98,26 @@ struct RxDeviceState_t {
     uint8_t a1Voltage = 0;
     uint8_t a2Voltage = 0;
     uint8_t flags = 0;
+    int16_t channels[16] = {};
+};
+
+struct QspConfiguration_t {
+    uint8_t protocolState = QSP_STATE_IDLE;
+    uint8_t crc = 0;
+    uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
+    uint8_t payloadLength = 0;
+    uint8_t frameToSend = 0;
+    uint8_t frameId = 0;
+    uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
+    uint32_t anyFrameRecivedAt = 0;
+    uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
+    void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
+    void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, RadioState_t*);
+    void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, RadioState_t*);    
+    bool canTransmit = false;
+    bool forcePongFrame = false;
+    uint8_t debugConfig = 0;
+    uint32_t frameDecodingStartedAt = 0;
+    uint32_t lastTxSlotTimestamp = 0;
+    bool transmitWindowOpen = false;
 };