diff --git a/.vscode/settings.json b/.vscode/settings.json index 96b3b58..af602ab 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -15,7 +15,19 @@ "numeric": "cpp", "*.tcc": "cpp", "bitset": "cpp", - "set": "cpp" + "set": "cpp", + "array": "cpp", + "utility": "cpp", + "__bit_reference": "cpp", + "__functional_base": "cpp", + "atomic": "cpp", + "chrono": "cpp", + "limits": "cpp", + "memory": "cpp", + "ratio": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "vector": "cpp" }, "files.exclude": { "**/build": true diff --git a/crossbow/crossbow.ino b/crossbow/crossbow.ino index 4109ff0..ab8161f 100644 --- a/crossbow/crossbow.ino +++ b/crossbow/crossbow.ino @@ -150,6 +150,16 @@ void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDev } +//I do not get function pointers to object methods, no way... +int getRcChannel_wrapper(uint8_t channel) { + return platformNode.getRcChannel(channel); +} + +//Same here, wrapper just works +void setRcChannel_wrapper(uint8_t channel, int value, int offset) { + platformNode.setRcChannel(channel, value, offset); +} + void setup(void) { #ifdef DEBUG_SERIAL @@ -158,11 +168,16 @@ void setup(void) qsp.onSuccessCallback = onQspSuccess; qsp.onFailureCallback = onQspFailure; + qsp.rcChannelGetCallback = getRcChannel_wrapper; + qsp.setRcChannelCallback = setRcChannel_wrapper; #ifdef DEVICE_MODE_RX platformNode.platformState = DEVICE_STATE_FAILSAFE; #else platformNode.platformState = DEVICE_STATE_OK; + + txInput.setRcChannelCallback = setRcChannel_wrapper; + #endif radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive); @@ -274,7 +289,7 @@ void loop(void) * This routine handles resync of TX/RX while hoppping frequencies * When not in bind mode. Bind mode is single frequency operation */ - if (!plaformNode.isBindMode) { + if (!platformNode.isBindMode) { radioNode.handleChannelDwell(); } @@ -314,7 +329,8 @@ void loop(void) radioNode.readAndDecode( &qsp, &rxDeviceState, - &txDeviceState + &txDeviceState, + platformNode.bindKey ); bool transmitPayload = false; @@ -408,7 +424,13 @@ void loop(void) break; case QSP_FRAME_RX_HEALTH: - encodeRxHealthPayload(&qsp, &rxDeviceState, radioNode.rssi, radioNode.snr); + encodeRxHealthPayload( + &qsp, + &rxDeviceState, + radioNode.rssi, + radioNode.snr, + platformNode.platformState + ); break; } @@ -420,7 +442,7 @@ void loop(void) if (currentMillis > sbusTime) { platformNode.setRcChannel(RSSI_CHANNEL - 1, rxDeviceState.indicatedRssi, 0); - sbusPreparePacket(sbusPacket, false, (platformNode.platformState == DEVICE_STATE_FAILSAFE)); + sbusPreparePacket(sbusPacket, false, (platformNode.platformState == DEVICE_STATE_FAILSAFE), getRcChannel_wrapper); Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); sbusTime = currentMillis + SBUS_UPDATE_RATE; } @@ -437,7 +459,7 @@ void loop(void) if (transmitPayload) { - radioNode.handleTx(&qsp); + radioNode.handleTx(&qsp, platformNode.bindKey); } #ifdef DEVICE_MODE_TX diff --git a/crossbow/qsp.cpp b/crossbow/qsp.cpp index 2e44a16..9f13280 100644 --- a/crossbow/qsp.cpp +++ b/crossbow/qsp.cpp @@ -3,16 +3,16 @@ void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) { - platformNode.setRcChannel(0, (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03), 1000); - platformNode.setRcChannel(1, (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F), 1000); - platformNode.setRcChannel(2, (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F), 1000); - platformNode.setRcChannel(3, (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF), 1000); - platformNode.setRcChannel(4, ((int) qsp->payload[5]) << 2, 1000); - platformNode.setRcChannel(5, ((int) qsp->payload[6]) << 2, 1000); - platformNode.setRcChannel(6, ((int) ((qsp->payload[7] >> 4) & 0b00001111)) << 6, 1000); - platformNode.setRcChannel(7, ((int) (qsp->payload[7] & 0b00001111)) << 6, 1000); - platformNode.setRcChannel(8, ((int) ((qsp->payload[8] >> 4) & 0b00001111)) << 6, 1000); - platformNode.setRcChannel(9, ((int) (qsp->payload[8] & 0b00001111)) << 6, 1000); + qsp->setRcChannelCallback(0, (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03), 1000); + qsp->setRcChannelCallback(1, (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F), 1000); + qsp->setRcChannelCallback(2, (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F), 1000); + qsp->setRcChannelCallback(3, (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF), 1000); + qsp->setRcChannelCallback(4, ((int) qsp->payload[5]) << 2, 1000); + qsp->setRcChannelCallback(5, ((int) qsp->payload[6]) << 2, 1000); + qsp->setRcChannelCallback(6, ((int) ((qsp->payload[7] >> 4) & 0b00001111)) << 6, 1000); + qsp->setRcChannelCallback(7, ((int) (qsp->payload[7] & 0b00001111)) << 6, 1000); + qsp->setRcChannelCallback(8, ((int) ((qsp->payload[8] >> 4) & 0b00001111)) << 6, 1000); + qsp->setRcChannelCallback(9, ((int) (qsp->payload[8] & 0b00001111)) << 6, 1000); } uint8_t get10bitHighShift(uint8_t channel) { @@ -41,7 +41,7 @@ void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte) qsp->crc = crc8_dvb_s2(qsp->crc, dataByte); } -void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr) { +void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr, uint8_t platformState) { qsp->payload[0] = rssi; qsp->payload[1] = snr; qsp->payload[2] = rxDeviceState->rxVoltage; @@ -50,7 +50,7 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta uint8_t flags = 0; - if (platformNode.platformState == DEVICE_STATE_FAILSAFE) { + if (platformState == DEVICE_STATE_FAILSAFE) { flags |= 0x01 << 0; } @@ -75,7 +75,7 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels) { for (uint8_t i = 0; i < noOfChannels; i++) { - int cV = constrain(platformNode.getRcChannel(i), 1000, 2000) - 1000; + int cV = constrain(qsp->rcChannelGetCallback(i), 1000, 2000) - 1000; uint16_t channelValue10 = cV & 0x03ff; uint8_t channelValue8 = (cV >> 2) & 0xff; @@ -133,10 +133,10 @@ void qspClearPayload(QspConfiguration_t *qsp) /** * Init CRC with salt based on 4 byte bind key */ -void qspInitCrc(QspConfiguration_t *qsp) { +void qspInitCrc(QspConfiguration_t *qsp, uint8_t bindKey[]) { qsp->crc = 0; for (uint8_t i = 0; i < 4; i++) { - qspComputeCrc(qsp, platformNode.bindKey[i]); + qspComputeCrc(qsp, bindKey[i]); } } @@ -144,7 +144,8 @@ void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + uint8_t bindKey[] ) { static uint8_t frameId; static uint8_t payloadLength; @@ -153,7 +154,7 @@ void qspDecodeIncomingFrame( if (qsp->protocolState == QSP_STATE_IDLE) { - qspInitCrc(qsp); + qspInitCrc(qsp, bindKey); qspClearPayload(qsp); receivedPayload = 0; qsp->frameDecodingStartedAt = millis(); @@ -201,9 +202,15 @@ void qspDecodeIncomingFrame( /** * Encode frame is corrent format and write to hardware */ -void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel) { +void qspEncodeFrame( + QspConfiguration_t *qsp, + uint8_t buffer[], + uint8_t *size, + uint8_t radioChannel, + uint8_t bindKey[] +) { //Salt CRC with bind key - qspInitCrc(qsp); + qspInitCrc(qsp, bindKey); //Write frame type and length // We are no longer sending payload length, so 4 bits are now free for other usages diff --git a/crossbow/qsp.h b/crossbow/qsp.h index 45fe66c..c23f18b 100644 --- a/crossbow/qsp.h +++ b/crossbow/qsp.h @@ -1,9 +1,6 @@ #include "Arduino.h" #include "variables.h" #include "radio_node.h" -#include "platform_node.h" - -extern PlatformNode platformNode; void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate); void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState); @@ -11,15 +8,16 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta 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, uint8_t rssi, uint8_t snr); +void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr, uint8_t platformState); void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels); void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + uint8_t bindKey[] ); void qspClearPayload(QspConfiguration_t *qsp); -void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel); +void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel, uint8_t bindKey[]); void encodePingPayload(QspConfiguration_t *qsp, uint32_t currentMicros); \ No newline at end of file diff --git a/crossbow/radio_node.cpp b/crossbow/radio_node.cpp index df2dfbf..5413c41 100644 --- a/crossbow/radio_node.cpp +++ b/crossbow/radio_node.cpp @@ -68,7 +68,8 @@ uint32_t RadioNode::getChannelEntryMillis(void) { void RadioNode::readAndDecode( QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + uint8_t bindKey[] ) { uint8_t tmpBuffer[MAX_PACKET_SIZE]; /* @@ -78,7 +79,7 @@ void RadioNode::readAndDecode( LoRa.read(tmpBuffer, bytesToRead); for (int i = 0; i < bytesToRead; i++) { - qspDecodeIncomingFrame(qsp, tmpBuffer[i], rxDeviceState, txDeviceState); + qspDecodeIncomingFrame(qsp, tmpBuffer[i], rxDeviceState, txDeviceState, bindKey); } //After reading, flush radio buffer, we have no need for whatever might be over there @@ -144,7 +145,7 @@ void RadioNode::handleTxDoneState(bool hop) { } } -void RadioNode::handleTx(QspConfiguration_t *qsp) { +void RadioNode::handleTx(QspConfiguration_t *qsp, uint8_t bindKey[]) { if (!canTransmit) { return; @@ -155,7 +156,7 @@ void RadioNode::handleTx(QspConfiguration_t *qsp) { LoRa.beginPacket(); //Prepare packet - qspEncodeFrame(qsp, tmpBuffer, &size, getChannel()); + qspEncodeFrame(qsp, tmpBuffer, &size, getChannel(), bindKey); //Sent it to radio in one SPI transaction LoRa.write(tmpBuffer, size); LoRa.endPacketAsync(); diff --git a/crossbow/radio_node.h b/crossbow/radio_node.h index 14b5f01..0d31a40 100644 --- a/crossbow/radio_node.h +++ b/crossbow/radio_node.h @@ -24,18 +24,19 @@ class RadioNode { void readAndDecode( QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, - TxDeviceState_t *txDeviceState + TxDeviceState_t *txDeviceState, + uint8_t bindKey[] ); uint8_t getChannel(void); uint32_t getChannelEntryMillis(void); void handleChannelDwell(void); void handleTxDoneState(bool hop); - void handleTx(QspConfiguration_t *qsp); + void handleTx(QspConfiguration_t *qsp, uint8_t bindKey[]); void set( uint8_t power, long bandwidth, uint8_t spreadingFactor, - uint8_t codingRate, + uint8_t codingRate, long frequency ); volatile int8_t bytesToRead = -1; diff --git a/crossbow/sbus.cpp b/crossbow/sbus.cpp index 732572c..0097e40 100644 --- a/crossbow/sbus.cpp +++ b/crossbow/sbus.cpp @@ -1,5 +1,4 @@ #include "Arduino.h" -#include "variables.h" #include "sbus.h" #define SBUS_MIN_OFFSET 173 @@ -31,7 +30,7 @@ int mapSbusToChannel(int in) { return (((long) in - 173l) * 1020l / 1638l) + 990; } -void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){ +void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe, int (* rcChannelGetCallback)(uint8_t)) { int output[SBUS_CHANNEL_NUMBER]; @@ -40,7 +39,7 @@ void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){ * 173-1811 with middle at 992 S.BUS protocol requires */ for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { - output[i] = mapChannelToSbus(platformNode.getRcChannel(i)); + output[i] = mapChannelToSbus(rcChannelGetCallback(i)); } uint8_t stateByte = 0x00; @@ -115,18 +114,18 @@ void SbusInput::recoverStuckFrames(void) } } -void sbusToChannels(byte buffer[]) { +void SbusInput::sbusToChannels(byte buffer[]) { - platformNode.setRcChannel(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0); - platformNode.setRcChannel(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0); - platformNode.setRcChannel(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0); - platformNode.setRcChannel(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0); - platformNode.setRcChannel(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0); - platformNode.setRcChannel(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0); - platformNode.setRcChannel(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0); - platformNode.setRcChannel(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0); - platformNode.setRcChannel(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0); - platformNode.setRcChannel(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0); + setRcChannelCallback(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0); + setRcChannelCallback(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0); + setRcChannelCallback(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0); + setRcChannelCallback(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0); + setRcChannelCallback(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0); + setRcChannelCallback(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0); + setRcChannelCallback(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0); + setRcChannelCallback(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0); + setRcChannelCallback(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0); + setRcChannelCallback(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0); //We use only 10 channels, so the reset can be just ignored } diff --git a/crossbow/sbus.h b/crossbow/sbus.h index 29ab03b..ca073aa 100644 --- a/crossbow/sbus.h +++ b/crossbow/sbus.h @@ -19,15 +19,17 @@ class SbusInput : public TxInput void loop(void); bool isReceiving(void); void recoverStuckFrames(void); + void (* setRcChannelCallback)(uint8_t channel, int value, int offset); private: HardwareSerial &_serial; uint32_t _frameDecodingStartedAt = 0; uint32_t _frameDecodingEndedAt = 0 ; uint8_t _protocolState = SBUS_DECODING_STATE_IDLE; - void sbusRead(void); + void sbusRead(void); + void sbusToChannels(byte buffer[]); }; -void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe); +void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe, int (* rcChannelGetCallback)(uint8_t)); #endif diff --git a/crossbow/variables.h b/crossbow/variables.h index 1ee74d0..c62d0e9 100644 --- a/crossbow/variables.h +++ b/crossbow/variables.h @@ -1,5 +1,6 @@ #include "Arduino.h" #include "radio_node.h" +#include "platform_node.h" #pragma once @@ -101,6 +102,8 @@ struct QspConfiguration_t { uint32_t anyFrameRecivedAt = 0; void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, uint8_t receivedChannel); void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*); + int (* rcChannelGetCallback)(uint8_t); + void (* setRcChannelCallback)(uint8_t channel, int value, int offset); bool forcePongFrame = false; uint32_t frameDecodingStartedAt = 0; uint32_t lastTxSlotTimestamp = 0;