diff --git a/crossbow.ino b/crossbow.ino index 30c9901..3cb5f1b 100644 --- a/crossbow.ino +++ b/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; } diff --git a/qsp.cpp b/qsp.cpp index 782f49c..3315d12 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,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 - - //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, 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 + 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 3553e0c..e80f72f 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,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); diff --git a/variables.h b/variables.h index 480f59d..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; @@ -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; }; \ No newline at end of file