From 9d609dfa77ef7375b594d22094873bd56291ee6f Mon Sep 17 00:00:00 2001 From: Pawel Spychalski Date: Thu, 16 Nov 2017 13:31:29 +0100 Subject: [PATCH 1/2] RX outout table moved to RxDeviceState object --- crossbow.ino | 14 ++++++-------- qsp.cpp | 7 +++---- qsp.h | 3 +-- variables.h | 1 + 4 files changed, 11 insertions(+), 14 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 30c9901..823843b 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,5 +1,5 @@ -#define DEVICE_MODE_TX -// #define DEVICE_MODE_RX +// #define DEVICE_MODE_TX +#define DEVICE_MODE_RX #define FEATURE_TX_OLED #define FORCE_TX_WITHOUT_INPUT @@ -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 @@ -123,7 +121,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 +215,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.bytesToRead = NO_DATA_TO_READ; @@ -283,7 +281,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 +320,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; } diff --git a/qsp.cpp b/qsp.cpp index 782f49c..a57422b 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -2,7 +2,7 @@ #include "variables.h" #include -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,7 +159,6 @@ void qspClearPayload(QspConfiguration_t *qsp) void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, - int ppm[], RxDeviceState_t *rxDeviceState, TxDeviceState_t *txDeviceState ) { @@ -228,7 +227,7 @@ void qspDecodeIncomingFrame( qsp->anyFrameRecivedAt = millis(); switch (frameId) { case QSP_FRAME_RC_DATA: - qspDecodeRcDataFrame(qsp, ppm); + qspDecodeRcDataFrame(qsp, rxDeviceState); break; case QSP_FRAME_RX_HEALTH: diff --git a/qsp.h b/qsp.h index 3553e0c..b891fae 100644 --- a/qsp.h +++ b/qsp.h @@ -1,7 +1,7 @@ #include "Arduino.h" #include -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,7 +12,6 @@ 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 ); diff --git a/variables.h b/variables.h index 480f59d..4e9c7ff 100644 --- a/variables.h +++ b/variables.h @@ -116,4 +116,5 @@ struct RxDeviceState_t { uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; uint8_t flags = 0; + int16_t channels[16] = {}; }; \ No newline at end of file From 3778fcd0cdbd6f9ae39f550cbfa051a26a61908b Mon Sep 17 00:00:00 2001 From: Pawel Spychalski Date: Thu, 16 Nov 2017 16:12:20 +0100 Subject: [PATCH 2/2] Callback for QSP decoding --- crossbow.ino | 52 +++++++++++++++++++++++++++++++++++++++++++++++++--- qsp.cpp | 51 ++++++--------------------------------------------- qsp.h | 3 ++- variables.h | 39 +++++++++++++++++++++------------------ 4 files changed, 78 insertions(+), 67 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 823843b..3cb5f1b 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,5 +1,5 @@ -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX #define FEATURE_TX_OLED #define FORCE_TX_WITHOUT_INPUT @@ -79,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 @@ -86,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; @@ -215,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], &rxDeviceState, &txDeviceState); + qspDecodeIncomingFrame(&qsp, radioState.data[i], &rxDeviceState, &txDeviceState, &radioState); } radioState.bytesToRead = NO_DATA_TO_READ; diff --git a/qsp.cpp b/qsp.cpp index a57422b..3315d12 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -160,7 +160,8 @@ void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + RadioState_t *radioState ) { static uint8_t frameId; static uint8_t payloadLength; @@ -189,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; @@ -216,49 +217,9 @@ void qspDecodeIncomingFrame( { if (qsp->crc == incomingByte) { //CRC is correct - - //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 (frameId < QSP_FRAME_COUNT) { - qsp->lastFrameReceivedAt[frameId] = millis(); - } - qsp->anyFrameRecivedAt = millis(); - switch (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; - } - else - { - //CRC failed, frame has to be rejected - //TODO do something in this case or something + qsp->onSuccessCallback(qsp, txDeviceState, rxDeviceState, radioState); + } else { + qsp->onFailureCallback(qsp, txDeviceState, rxDeviceState, radioState); } // In both cases switch to listening for next preamble diff --git a/qsp.h b/qsp.h index b891fae..e80f72f 100644 --- a/qsp.h +++ b/qsp.h @@ -13,7 +13,8 @@ void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + RadioState_t *radioState ); void qspClearPayload(QspConfiguration_t *qsp); void qspEncodeFrame(QspConfiguration_t *qsp); diff --git a/variables.h b/variables.h index 4e9c7ff..7ed84c1 100644 --- a/variables.h +++ b/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; @@ -117,4 +99,25 @@ struct RxDeviceState_t { 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; }; \ No newline at end of file