From 021decff98bdebf7b7d2cb99e8ea46418a285616 Mon Sep 17 00:00:00 2001
From: Pawel Spychalski <pawel.spychalski@raisin.com>
Date: Thu, 12 Oct 2017 13:29:10 +0200
Subject: [PATCH] Rx_health frame encoding, no data is yet acquired from Rx
 hardware

---
 README.md    | 15 ++++++++-------
 crossbow.ino | 21 +++++++++++++++++++--
 qsp.cpp      | 30 ++++++++++++++++++++++--------
 qsp.h        |  1 +
 variables.h  | 12 +++++++++++-
 5 files changed, 61 insertions(+), 18 deletions(-)

diff --git a/README.md b/README.md
index ca64cc7..ed08d5a 100644
--- a/README.md
+++ b/README.md
@@ -39,10 +39,11 @@ Total length of `RC_DATA` payload is 9 bytes
 
 ### `RX_HEALTH` frame format
 
-| Byte | Description                        |
-| ---- | ----                               |
-| 0    | RX RSSI                            |
-| 1    | RX SNR                            |
-| 2    | RX supply volatage, sent in 0,1V   |
-| 3    | RX analog input 1 sent in 0,1V     |
-| 4    | RX analog input 2 sent in 0,1V     |
+| Byte  | Description                           |
+| ----  | ----                                  |
+| 1     | RX RSSI                               |
+| 2     | RX SNR                                |   
+| 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               |
diff --git a/crossbow.ino b/crossbow.ino
index eddcb19..f49ef90 100644
--- a/crossbow.ino
+++ b/crossbow.ino
@@ -52,12 +52,14 @@ bool canTransmit = false;
  * Start of QSP protocol implementation
  */
 QspConfiguration_t qsp = {};
+RxDeviceState_t rxDeviceState = {};
 
 /*
  * End of QSP protocol implementation
  */
 
 static uint32_t lastRcFrameTransmit = 0;
+static uint32_t lastRxHealthFrameTransmit = 0;
 
 /*
  * Serial port used to send data
@@ -250,8 +252,6 @@ void loop(void)
     bool transmitPayload = false;
 
 #ifdef DEVICE_MODE_TX
-
-    //TODO It should be only possible to transmit when radio is not receiveing
     /*
      * RC_DATA QSP frame
      */
@@ -268,6 +268,23 @@ void loop(void)
 
 #endif
 
+#ifdef DEVICE_MODE_RX
+    /*
+     * RC_DATA QSP frame
+     */
+    if (currentMillis - lastRxHealthFrameTransmit > RX_RX_HEALTH_FRAME_RATE && !transmitPayload && qsp.protocolState == QSP_STATE_IDLE)
+    {
+        lastRxHealthFrameTransmit = currentMillis;
+
+        qspClearPayload(&qsp);
+        encodeRxHealthPayload(&qsp, &rxDeviceState);
+        qsp.frameToSend = QSP_FRAME_RX_HEALTH;
+
+        transmitPayload = true;
+    }
+
+#endif
+
 #ifdef LORA_HARDWARE_SERIAL
     if (Serial.available())
     {
diff --git a/qsp.cpp b/qsp.cpp
index 2061d8b..9fa35c3 100644
--- a/qsp.cpp
+++ b/qsp.cpp
@@ -54,6 +54,17 @@ void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte)
     qsp->crc ^= dataByte;
 }
 
+void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState) {
+    qsp->payload[0] = rxDeviceState->rssi;
+    qsp->payload[1] = constrain(rxDeviceState->snr * 10, 0, 255);
+    qsp->payload[2] = rxDeviceState->rxVoltage;
+    qsp->payload[3] = rxDeviceState->a1Voltage;
+    qsp->payload[4] = rxDeviceState->a2Voltage;
+    qsp->payload[5] = qsp->lastReceivedPacketId;
+
+    qsp->payloadLength = 6;
+}
+
 /**
  * Encode 10 RC channels 
  */
@@ -130,6 +141,7 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
 
     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;
@@ -199,15 +211,17 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
                 qsp->lastFrameReceivedAt[frameId] = millis();
             }
 
-            switch (frameId) {
-            case QSP_FRAME_RC_DATA:
-                qspDecodeRcDataFrame(qsp, ppm);
-                break;
+            qsp->lastReceivedPacketId = packetId;
 
-            default:
-                //Unknown frame
-                //TODO do something in this case
-                break;
+            switch (frameId) {
+                case QSP_FRAME_RC_DATA:
+                    qspDecodeRcDataFrame(qsp, ppm);
+                    break;
+
+                default:
+                    //Unknown frame
+                    //TODO do something in this case
+                    break;
             }
         }
         else
diff --git a/qsp.h b/qsp.h
index 868a5ef..e581815 100644
--- a/qsp.h
+++ b/qsp.h
@@ -5,6 +5,7 @@ void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]);
 uint8_t get10bitHighShift(uint8_t channel);
 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(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[]);
diff --git a/variables.h b/variables.h
index 0f6a98f..0f5ac00 100644
--- a/variables.h
+++ b/variables.h
@@ -6,7 +6,8 @@
 
 #define PPM_CHANNEL_COUNT 10
 
-#define TX_RC_FRAME_RATE 5000 //ms
+#define RX_RX_HEALTH_FRAME_RATE 5000
+#define TX_RC_FRAME_RATE 1000 //ms
 #define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 4)
 
 #define CHANNEL_ID 0x01
@@ -55,4 +56,13 @@ struct QspConfiguration_t {
     uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
     uint8_t deviceState = DEVICE_STATE_OK;
     void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
+    uint8_t lastReceivedPacketId = 0;
+};
+
+struct RxDeviceState_t {
+    int rssi = 0;
+    float snr = 0;
+    uint8_t rxVoltage = 0;
+    uint8_t a1Voltage = 0;
+    uint8_t a2Voltage = 0;
 };
\ No newline at end of file