diff --git a/.vscode/arduino.json b/.vscode/arduino.json index e0b924b..8b7bafb 100644 --- a/.vscode/arduino.json +++ b/.vscode/arduino.json @@ -1,6 +1,6 @@ { "board": "bsfrance:avr:lora32u4", "sketch": "crossbow.ino", - "port": "COM17", + "port": "COM15", "output": "./build" } \ No newline at end of file diff --git a/README.md b/README.md index e101224..28d5bab 100644 --- a/README.md +++ b/README.md @@ -13,9 +13,8 @@ Development, not yet functional | 1 | Preamble | "Q" 0x51 | | 2 | Channel ID | channel used for comunication between TX and RX | | 3 | Frame type & Length | bits 7-5 defines frame, bits 4-0 payload length | -| 4 | Packet ID | | | 5 - 36 | Payload | 32 bytes max | -| payload length + 5 | CRC | XOR of all previous bytes | +| payload length + 4 | CRC | XOR of all previous bytes | ## Frame types @@ -26,8 +25,8 @@ Development, not yet functional | 0010 | 0x2 | Request receiver configuration | TX -> RX | | 0011 | 0x3 | Receiver configuration | RX -> TX | | 0100 | 0x4 | Set receiver configuration | TX -> RX | -| 0101 | 0x5 | PING frame | TX -> RX | -| 0110 | 0x6 | PONG frame | RX -> TX | +| 0101 | 0x5 | PING frame, uses 9 byte payload | TX -> RX | +| 0110 | 0x6 | PONG frame, the same payload as PING | RX -> TX | ### `RC_DATA` frame format @@ -48,7 +47,14 @@ Total length of `RC_DATA` payload is 9 bytes | 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 | +| 6 | Flags | + +#### Flags + +| Bit | Meaning | +| ---- | ---- | +| 00000001 | Device in Failsafe mode | + ### `PING` and `PONG` frames diff --git a/crossbow.ino b/crossbow.ino index d5a2926..661d7d0 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,38 +1,43 @@ -#define LORA_HARDWARE_SPI - // #define DEVICE_MODE_TX #define DEVICE_MODE_RX +#define FEATURE_TX_OLED + // #define DEBUG_SERIAL // #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL #include -// #include #include "variables.h" -#include "sbus.h" #include "qsp.h" +volatile int ppm[16] = {0}; + // LoRa32u4 ports #define LORA32U4_SS_PIN 8 #define LORA32U4_RST_PIN 4 #define LORA32U4_DI0_PIN 7 -int ppm[16] = {0}; - /* * Main defines for device working in TX mode */ #ifdef DEVICE_MODE_TX - -// #define OLED_RESET -1 #include -// #include - PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true); -// PPMReader ppmReader(11, 2, MODE_PIN_CHANGE_INTERRUPT); -// Adafruit_SSD1306 display(OLED_RESET); + +#include "txbuzzer.h" + +BuzzerState_t buzzer; + +#ifdef FEATURE_TX_OLED + +#define OLED_RESET -1 +#include +Adafruit_SSD1306 display(OLED_RESET); +uint32_t lastOledTaskTime = 0; + +#endif #endif @@ -40,6 +45,9 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true); * Main defines for device working in RX mode */ #ifdef DEVICE_MODE_RX + + #include "sbus.h" + uint32_t sbusTime = 0; uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0}; uint32_t lastRxStateTaskTime = 0; @@ -48,10 +56,10 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true); /* * Start of QSP protocol implementation */ -QspConfiguration_t qsp = {}; -RxDeviceState_t rxDeviceState = {}; +volatile QspConfiguration_t qsp = {}; +volatile RxDeviceState_t rxDeviceState = {}; +volatile TxDeviceState_t txDeviceState = {}; -#ifdef LORA_HARDWARE_SPI uint8_t getRadioRssi(void) { @@ -59,9 +67,9 @@ uint8_t getRadioRssi(void) return map(constrain(LoRa.packetRssi() * -1, 0, 164), 0, 164, 255, 0); } -float getRadioSnr(void) +uint8_t getRadioSnr(void) { - return (uint8_t) constrain(LoRa.packetSnr() * 10, 0, 255); + return (uint8_t) constrain(LoRa.packetSnr(), 0, 255); } void radioPacketStart(void) @@ -85,16 +93,6 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp) LoRa.write(dataByte); } -#endif - -/* -display.clearDisplay(); -display.setCursor(0,0); -display.print("Lat:"); -display.print(remoteData.latitude); -display.display(); -*/ - void setup(void) { #ifdef DEBUG_SERIAL @@ -109,8 +107,6 @@ void setup(void) qsp.deviceState = DEVICE_STATE_OK; #endif -#ifdef LORA_HARDWARE_SPI - #ifdef WAIT_FOR_SERIAL while (!Serial) { ; // wait for serial port to connect. Needed for native USB @@ -134,24 +130,14 @@ void setup(void) while (true); } - LoRa.setSignalBandwidth(250E3); - LoRa.setSpreadingFactor(7); - LoRa.setCodingRate4(5); - + LoRa.setSignalBandwidth(500E3); + LoRa.setSpreadingFactor(8); + LoRa.setCodingRate4(6); + LoRa.enableCrc(); LoRa.onReceive(onReceive); LoRa.receive(); -#endif #ifdef DEVICE_MODE_RX - /* - * Initialize OLED display - */ - // display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) - // display.setTextSize(1); - // display.setTextColor(WHITE); - // display.clearDisplay(); - // display.display(); - //initiallize default ppm values for (int i = 0; i < 16; i++) { @@ -169,17 +155,28 @@ void setup(void) TCCR1A = 0; //reset timer1 TCCR1B = 0; TCCR1B |= (1 << CS11); //set timer1 to increment every 0,5 us or 1us on 8MHz + +#ifdef FEATURE_TX_OLED + display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) + display.setTextSize(1); + display.setTextColor(WHITE); + display.clearDisplay(); + display.display(); +#endif + + /* + * TX should start talking imediately after power up + */ + qsp.canTransmit = true; + + pinMode(TX_BUZZER_PIN, OUTPUT); + + //Play single tune to indicate power up + buzzerSingleMode(BUZZER_MODE_CHIRP, &buzzer); #endif pinMode(LED_BUILTIN, OUTPUT); -/* - * TX should start talking imediately after power up - */ -#ifdef DEVICE_MODE_TX - qsp.canTransmit = true; -#endif - #ifdef DEBUG_SERIAL qsp.debugConfig |= DEBUG_FLAG_SERIAL; #endif @@ -189,121 +186,294 @@ void setup(void) } +int8_t txSendSequence[16] = { + QSP_FRAME_PING, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA, + QSP_FRAME_RC_DATA +}; + +int8_t rxSendSequence[16] = { + QSP_FRAME_RX_HEALTH, + -1, + -1, + -1, + QSP_FRAME_RX_HEALTH, + -1, + -1, + -1, + QSP_FRAME_RX_HEALTH, + -1, + -1, + -1, + QSP_FRAME_RX_HEALTH, + -1, + -1, + -1 +}; + +uint8_t currentSequenceIndex = 0; +#define TRANSMIT_SEQUENCE_COUNT 16 + #ifdef DEVICE_MODE_RX void updateRxDeviceState(RxDeviceState_t *rxDeviceState) { rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255); rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255); rxDeviceState->a2Voltage = map(analogRead(RX_ADC_PIN_3), 0, 1024, 0, 255); - rxDeviceState->rssi = getRadioRssi(); - rxDeviceState->snr = getRadioSnr(); } +int8_t getFrameToTransmit(QspConfiguration_t *qsp) { + + if (qsp->forcePongFrame) { + qsp->forcePongFrame = false; + return QSP_FRAME_PONG; + } + + int8_t retVal = rxSendSequence[currentSequenceIndex]; + + currentSequenceIndex++; + if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) { + currentSequenceIndex = 0; + } + + return retVal; +} + +#endif + +#ifdef DEVICE_MODE_TX +int8_t getFrameToTransmit(QspConfiguration_t *qsp) { + int8_t retVal = txSendSequence[currentSequenceIndex]; + + currentSequenceIndex++; + if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) { + currentSequenceIndex = 0; + } + + return retVal; +} #endif void loop(void) { + +#ifdef DEVICE_MODE_TX + if (txDeviceState.readPacket) { + int incomingByte = LoRa.read(); + if (incomingByte > -1) { + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState); + } else { + txDeviceState.rssi = getRadioRssi(); + txDeviceState.snr = getRadioSnr(); + txDeviceState.readPacket = false; + } + } +#endif + uint32_t currentMillis = millis(); bool transmitPayload = false; + static uint32_t previousAnyFrameReceivedAt = 0; /* * Watchdog for frame decoding stuck somewhere in the middle of a process */ if ( qsp.protocolState != QSP_STATE_IDLE && - abs(currentMillis - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME + qsp.frameDecodingStartedAt + QSP_MAX_FRAME_DECODE_TIME < currentMillis ) { qsp.protocolState = QSP_STATE_IDLE; } - if ( - qsp.forcePongFrame && - !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE - ) - { - qsp.forcePongFrame = false; - qsp.lastFrameTransmitedAt[QSP_FRAME_PONG] = currentMillis; - qsp.frameToSend = QSP_FRAME_PONG; - transmitPayload = true; - } - - #ifdef DEVICE_MODE_TX -#ifdef DEBUG_PING_PONG - //PING frame if ( - currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_PING] > TX_PING_RATE && - !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE - ) - { - qsp.lastFrameTransmitedAt[QSP_FRAME_PING] = currentMillis; - - qspClearPayload(&qsp); - encodePingPayload(&qsp, micros()); - qsp.frameToSend = QSP_FRAME_PING; + qsp.protocolState == QSP_STATE_IDLE && + qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis + ) { - transmitPayload = true; - } -#endif + int8_t frameToSend = getFrameToTransmit(&qsp); - /* - * RC_DATA QSP frame - */ - if ( - currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] > TX_RC_FRAME_RATE && - !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE - ) - { - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] = currentMillis; + if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) { + frameToSend = -1; + //FIXME uncomment to enable full Failsafe + } - qspClearPayload(&qsp); - encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT); - qsp.frameToSend = QSP_FRAME_RC_DATA; + if (frameToSend > -1) { - transmitPayload = true; + qsp.frameToSend = frameToSend; + qspClearPayload(&qsp); + + switch (qsp.frameToSend) { + case QSP_FRAME_PING: + encodePingPayload(&qsp, micros()); + break; + + case QSP_FRAME_RC_DATA: + encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT); + break; + } + + transmitPayload = true; + } + + qsp.lastTxSlotTimestamp = currentMillis; } #endif #ifdef DEVICE_MODE_RX - if (currentMillis > sbusTime) { - sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); - Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); - - sbusTime = currentMillis + SBUS_UPDATE_RATE; - } - /* * This routine updates RX device state and updates one of radio channels with RSSI value */ - if (currentMillis - lastRxStateTaskTime > RX_TASK_HEALTH) { + if (lastRxStateTaskTime + RX_TASK_HEALTH < currentMillis) { lastRxStateTaskTime = currentMillis; updateRxDeviceState(&rxDeviceState); ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 255, 1000, 2000); + if (qsp.deviceState == DEVICE_STATE_FAILSAFE) { + digitalWrite(LED_BUILTIN, HIGH); + } else { + digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); + } } - + /* - * RX_HEALTH QSP frame + * Main routine to answer to TX module */ - if ( - currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] > RX_RX_HEALTH_FRAME_RATE && - !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE - ) - { - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] = currentMillis; - qspClearPayload(&qsp); - encodeRxHealthPayload(&qsp, &rxDeviceState); - qsp.frameToSend = QSP_FRAME_RX_HEALTH; + if (qsp.transmitWindowOpen && qsp.protocolState == QSP_STATE_IDLE) { + qsp.transmitWindowOpen = false; + + int8_t frameToSend = getFrameToTransmit(&qsp); + if (frameToSend > -1) { + qsp.frameToSend = frameToSend; + + if (frameToSend != QSP_FRAME_PONG) { + qspClearPayload(&qsp); + } + switch (qsp.frameToSend) { + case QSP_FRAME_PONG: + /* + * Pong frame just responses with received payload + */ + break; + + case QSP_FRAME_RX_HEALTH: + encodeRxHealthPayload(&qsp, &rxDeviceState); + break; + } + + transmitPayload = true; + } - transmitPayload = true; } + if (currentMillis > sbusTime) { + sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); + Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); + sbusTime = currentMillis + SBUS_UPDATE_RATE; + } + + if (qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA] + RX_FAILSAFE_DELAY < currentMillis) { + qsp.deviceState = DEVICE_STATE_FAILSAFE; + } else { + qsp.deviceState = DEVICE_STATE_OK; + } + +#endif + +#ifdef DEVICE_MODE_TX + + buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer); + + // This routing enables when TX starts to receive signal from RX for a first time or after + // failsafe + if (txDeviceState.isReceiving == false && qsp.anyFrameRecivedAt != 0) { + //TX module started to receive data + buzzerSingleMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer); + txDeviceState.isReceiving = true; + qsp.deviceState = DEVICE_STATE_OK; + } + + //Here we detect failsafe state on TX module + if ( + txDeviceState.isReceiving && + qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis + ) { + txDeviceState.isReceiving = false; + rxDeviceState.a1Voltage = 0; + rxDeviceState.a2Voltage = 0; + rxDeviceState.rxVoltage = 0; + rxDeviceState.rssi = 0; + rxDeviceState.snr = 0; + rxDeviceState.flags = 0; + txDeviceState.roundtrip = 0; + qsp.deviceState = DEVICE_STATE_FAILSAFE; + qsp.anyFrameRecivedAt = 0; + } + + //FIXME rxDeviceState should be resetted also in RC_HEALT frame is not received in a long period + + //Handle audible alarms + if (qsp.deviceState == DEVICE_STATE_FAILSAFE) { + //Failsafe detected by TX + buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer); + } else if (txDeviceState.isReceiving && (rxDeviceState.flags & 0x1) == 1) { + //Failsafe reported by RX module + buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer); + } else if (txDeviceState.isReceiving && txDeviceState.rssi < 100) { + buzzerContinousMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer); + } else if (txDeviceState.isReceiving && txDeviceState.rssi < 128) { + buzzerContinousMode(BUZZER_MODE_CHIRP, &buzzer); + } else { + buzzerContinousMode(BUZZER_MODE_OFF, &buzzer); + } + +#ifdef FEATURE_TX_OLED + if ( + currentMillis - lastOledTaskTime > OLED_UPDATE_RATE + ) { + lastOledTaskTime = currentMillis; + + display.clearDisplay(); + + display.setTextColor(WHITE, BLACK); + display.setCursor(0, 0); + display.setTextSize(3); + display.print(map(txDeviceState.rssi, 0, 255, 0, 164)); + + display.setCursor(18, 28); + display.setTextSize(2); + display.print(txDeviceState.snr); + + display.setCursor(74, 0); + display.setTextSize(3); + display.print(map(rxDeviceState.rssi, 0, 255, 0, 164)); + + display.setCursor(92, 28); + display.setTextSize(2); + display.print(rxDeviceState.snr); + + display.setCursor(54, 48); + display.setTextSize(2); + display.print(txDeviceState.roundtrip); + + display.display(); + } +#endif + #endif if (qsp.canTransmit && transmitPayload) @@ -311,40 +481,30 @@ void loop(void) radioPacketStart(); qspEncodeFrame(&qsp); radioPacketEnd(); - - #ifdef DEBUG_LED - digitalWrite(LED_BUILTIN, HIGH); - delay(10); - digitalWrite(LED_BUILTIN, LOW); - delay(70); - digitalWrite(LED_BUILTIN, HIGH); - delay(10); - digitalWrite(LED_BUILTIN, LOW); - #endif transmitPayload = false; } - /* - * Here we do state handling and similar operations - */ -#ifdef DEVICE_MODE_RX - if (abs(currentMillis - qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA]) > RX_FAILSAFE_DELAY) { - qsp.deviceState = DEVICE_STATE_FAILSAFE; - } else { - qsp.deviceState = DEVICE_STATE_OK; - } -#endif } -#ifdef LORA_HARDWARE_SPI void onReceive(int packetSize) { if (packetSize == 0) return; - while (LoRa.available()) +#ifdef DEVICE_MODE_TX + txDeviceState.readPacket = true; +#endif + +#ifdef DEVICE_MODE_RX + + int incomingByte; + + while (incomingByte = LoRa.read(), incomingByte > -1) { - qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState); } -} -#endif \ No newline at end of file + + rxDeviceState.rssi = getRadioRssi(); + rxDeviceState.snr = getRadioSnr(); +#endif +} \ No newline at end of file diff --git a/qsp.cpp b/qsp.cpp index fde358c..44d733d 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -60,7 +60,14 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta qsp->payload[2] = rxDeviceState->rxVoltage; qsp->payload[3] = rxDeviceState->a1Voltage; qsp->payload[4] = rxDeviceState->a2Voltage; - qsp->payload[5] = qsp->lastReceivedPacketId; + + uint8_t flags = 0; + + if (qsp->deviceState == DEVICE_STATE_FAILSAFE) { + flags |= 0x01 << 0; + } + + qsp->payload[5] = flags; qsp->payloadLength = 6; } @@ -71,7 +78,7 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta rxDeviceState->rxVoltage = qsp->payload[2]; rxDeviceState->a1Voltage = qsp->payload[3]; rxDeviceState->a2Voltage = qsp->payload[4]; - // rxDeviceState->rssi = qsp->payload[0]; //TODO we skipped decoding this byte, figure it out + rxDeviceState->flags = qsp->payload[5]; } /** @@ -127,13 +134,6 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t qsp->payloadLength = 9; } -uint8_t qspGetPacketId() -{ - static uint8_t packetId = 0; - - return packetId++; -} - void qspClearPayload(QspConfiguration_t *qsp) { for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) @@ -143,16 +143,19 @@ void qspClearPayload(QspConfiguration_t *qsp) qsp->payloadLength = 0; } -void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[], RxDeviceState_t *rxDeviceState) -{ +void qspDecodeIncomingFrame( + QspConfiguration_t *qsp, + uint8_t incomingByte, + int ppm[], + RxDeviceState_t *rxDeviceState, + TxDeviceState_t *txDeviceState +) { static uint8_t frameId; static uint8_t payloadLength; static uint8_t receivedPayload; - static uint8_t packetId; //TODO move this to global scope maybe? 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; @@ -169,7 +172,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p qspClearPayload(qsp); receivedPayload = 0; - packetId = 0; } else { @@ -188,12 +190,9 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p } else if (qsp->protocolState == QSP_STATE_FRAME_TYPE_RECEIVED) { - qsp->crc ^= incomingByte; - packetId = incomingByte; - qsp->protocolState = QSP_STATE_PACKET_ID_RECEIVED; - } - else if (qsp->protocolState == QSP_STATE_PACKET_ID_RECEIVED) - { + if (receivedPayload >= QSP_PAYLOAD_LENGTH) { + qsp->protocolState = QSP_STATE_IDLE; + } //Now it's time for payload qsp->crc ^= incomingByte; @@ -209,7 +208,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p } else if (qsp->protocolState == QSP_STATE_PAYLOAD_RECEIVED) { - if (qsp->crc == incomingByte) { //CRC is correct @@ -220,22 +218,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p if (frameId < QSP_FRAME_COUNT) { qsp->lastFrameReceivedAt[frameId] = millis(); } + qsp->anyFrameRecivedAt = millis(); - qsp->lastReceivedPacketId = packetId; - - if (qsp->debugConfig & DEBUG_FLAG_SERIAL) { - Serial.print("Frame "); - Serial.print(frameId); - Serial.println(" received"); - } - - if (qsp->debugConfig & DEBUG_FLAG_LED) { - digitalWrite(LED_BUILTIN, HIGH); - delay(10); - digitalWrite(LED_BUILTIN, LOW); - delay(100); - } - switch (frameId) { case QSP_FRAME_RC_DATA: qspDecodeRcDataFrame(qsp, ppm); @@ -243,12 +227,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p case QSP_FRAME_RX_HEALTH: decodeRxHealthPayload(qsp, rxDeviceState); - if (qsp->debugConfig & DEBUG_FLAG_SERIAL) { - Serial.print("RX RSSI: "); - Serial.println(rxDeviceState->rssi); - Serial.print("RX SNR: "); - Serial.println(rxDeviceState->snr); - } break; case QSP_FRAME_PING: @@ -256,18 +234,13 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p break; case QSP_FRAME_PONG: - if (qsp->debugConfig & DEBUG_FLAG_SERIAL) { + 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; - uint32_t incoming = 0; + txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000; - incoming = qsp->payload[0]; - incoming += (uint32_t) qsp->payload[1] << 8; - incoming += (uint32_t) qsp->payload[2] << 16; - incoming += (uint32_t) qsp->payload[3] << 24; - - Serial.print("Rountrip: "); - Serial.println((micros() - incoming) / 1000); - } break; default: @@ -275,6 +248,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p //TODO do something in this case break; } + + qsp->transmitWindowOpen = true; } else { @@ -304,9 +279,6 @@ void qspEncodeFrame(QspConfiguration_t *qsp) { data |= (qsp->frameToSend << 4) & 0xf0; qsp->hardwareWriteFunction(data, qsp); - //Write packet ID - qsp->hardwareWriteFunction(qspGetPacketId(), qsp); - //Write payload for (uint8_t i = 0; i < qsp->payloadLength; i++) { diff --git a/qsp.h b/qsp.h index aa60e94..b801ae0 100644 --- a/qsp.h +++ b/qsp.h @@ -9,8 +9,13 @@ 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[], RxDeviceState_t *rxDeviceState); +void qspDecodeIncomingFrame( + QspConfiguration_t *qsp, + uint8_t incomingByte, + int ppm[], + RxDeviceState_t *rxDeviceState, + TxDeviceState_t *txDeviceState +); void qspClearPayload(QspConfiguration_t *qsp); void qspEncodeFrame(QspConfiguration_t *qsp); diff --git a/sbus.cpp b/sbus.cpp index 53ffe0c..f8ebff6 100644 --- a/sbus.cpp +++ b/sbus.cpp @@ -12,6 +12,15 @@ #define SBUS_STATE_FAILSAFE 0x08 #define SBUS_STATE_SIGNALLOSS 0x04 +/* +Precomputed mapping from 990-2010 to 173:1811 +equivalent to +map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET); +*/ +int mapChannelToSbus(int in) { + return (((long) in * 1605l) / 1000l) - 1417; +} + void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){ static int output[SBUS_CHANNEL_NUMBER] = {0}; @@ -21,7 +30,7 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool * 173-1811 with middle at 992 S.BUS protocol requires */ for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { - output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET); + output[i] = mapChannelToSbus(channels[i]); } uint8_t stateByte = 0x00; diff --git a/track1.txt b/track1.txt new file mode 100644 index 0000000..77ba353 --- /dev/null +++ b/track1.txt @@ -0,0 +1,31 @@ +2 +3 +4 +5 +12 +13 +12 +13 +12 +13 +12 +13 +12 +13 +12 +13 +12 +13 +12 +13 +12 +13 +6 +7 +12 +13 +12 +13 +12 +13 +2 \ No newline at end of file diff --git a/track2.txt b/track2.txt new file mode 100644 index 0000000..216572c --- /dev/null +++ b/track2.txt @@ -0,0 +1,14 @@ +13 +12 +13 +6 +7 +12 +13 +12 +13 +12 +13 +12 +13 +2 \ No newline at end of file diff --git a/track3.txt b/track3.txt new file mode 100644 index 0000000..d954092 --- /dev/null +++ b/track3.txt @@ -0,0 +1,10 @@ +12 +13 +12 +13 +2 +21 +22 +23 +24 +25 \ No newline at end of file diff --git a/txbuzzer.cpp b/txbuzzer.cpp new file mode 100644 index 0000000..5994977 --- /dev/null +++ b/txbuzzer.cpp @@ -0,0 +1,66 @@ +#include "Arduino.h" +#include "txbuzzer.h" + +/** + * This method plays selected pattern only once + * It disables continious mode + */ +void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer) { + buzzer->singleModeEnabled = true; + buzzer->enabled = false; + buzzer->mode = mode; + buzzer->tick = 0; +} + +void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer) { + buzzer->singleModeEnabled = false; + buzzer->enabled = true; + buzzer->mode = mode; +} + +void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) +{ + + if (!buzzer->enabled && !buzzer->singleModeEnabled) + { + digitalWrite(pin, LOW); + return; + } + + if (timestamp > buzzer->updateTime) + { + + int8_t currentPattern = buzzer->pattern[buzzer->mode][buzzer->element]; + + if (currentPattern == PATTERN_CYCLE_OFF) + { + digitalWrite(pin, LOW); + } + else if (currentPattern == PATTERN_CYCLE_ON) + { + digitalWrite(pin, HIGH); + } + else if (currentPattern == PATTERN_CYCLE_IGNORE || currentPattern == buzzer->tick) + { + + if (currentPattern != PATTERN_CYCLE_IGNORE) + { + digitalWrite(pin, !digitalRead(pin)); + } + + buzzer->element++; + if (buzzer->element == PATTERN_ELEMENT_NUMBER) + { + buzzer->element = 0; + } + } + + buzzer->tick++; + if (buzzer->tick >= buzzer->patternMaxTick) + { + buzzer->tick = 0; + buzzer->singleModeEnabled = false; + } + buzzer->updateTime = timestamp + buzzer->patternTickPerdiod; + } +}; \ No newline at end of file diff --git a/txbuzzer.h b/txbuzzer.h new file mode 100644 index 0000000..aa2bb45 --- /dev/null +++ b/txbuzzer.h @@ -0,0 +1,46 @@ +#pragma once + +#include "Arduino.h" + +#define PATTERN_CYCLE_OFF 127 +#define PATTERN_CYCLE_ON -1 +#define PATTERN_CYCLE_IGNORE -2 + +#define PATTERN_MODES_NUMBER 6 +#define PATTERN_ELEMENT_NUMBER 4 + +enum { + BUZZER_MODE_OFF = 0, + BUZZER_MODE_CONTINUOUS = 1, + BUZZER_MODE_SLOW_BEEP = 2, + BUZZER_MODE_FAST_BEEP = 3, + BUZZER_MODE_CHIRP = 4, + BUZZER_MODE_DOUBLE_CHIRP = 5 +}; + +struct BuzzerState_t { + bool enabled = false; //Continous mode buzzer + bool singleModeEnabled = false; + uint8_t mode = BUZZER_MODE_OFF; + + uint32_t updateTime = 0; + + uint8_t tick = 0; + uint8_t element = 0; + const uint8_t patternMaxTick = 20; + const uint8_t patternTickPerdiod = 75; + + const int8_t pattern[PATTERN_MODES_NUMBER][PATTERN_ELEMENT_NUMBER] = { + {PATTERN_CYCLE_OFF}, + {PATTERN_CYCLE_ON}, + {0, 7, 10, 17}, + {0, 4, 10, 14}, + {0, 1, PATTERN_CYCLE_IGNORE, PATTERN_CYCLE_IGNORE}, + {0, 1, 2, 3} + }; + +}; + +void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer); +void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer); +void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); \ No newline at end of file diff --git a/variables.h b/variables.h index 94797cc..ceacb2c 100644 --- a/variables.h +++ b/variables.h @@ -1,5 +1,7 @@ #pragma once +#define OLED_UPDATE_RATE 500 + #define SBUS_UPDATE_RATE 15 //ms #define SBUS_PACKET_LENGTH 25 @@ -9,17 +11,15 @@ #define RX_TASK_HEALTH 200 //5Hz should be enough #define RSSI_CHANNEL 11 -#define RX_RX_HEALTH_FRAME_RATE 1000 -#define TX_RC_FRAME_RATE 500 //ms -#define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8) - -#define TX_PING_RATE 2000 +#define TX_TRANSMIT_SLOT_RATE 70 //ms +#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8) +#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4) #define CHANNEL_ID 0x01 #define QSP_PREAMBLE 0x51 #define QSP_PAYLOAD_LENGTH 32 -#define QSP_MAX_FRAME_DECODE_TIME 50 //max time that frame can be decoded in ms +#define QSP_MAX_FRAME_DECODE_TIME 10 //max time that frame can be decoded in ms #define QSP_FRAME_RC_DATA 0x0 #define QSP_FRAME_RX_HEALTH 0x1 @@ -39,14 +39,14 @@ enum dataStates { QSP_STATE_PREAMBLE_RECEIVED, QSP_STATE_CHANNEL_RECEIVED, QSP_STATE_FRAME_TYPE_RECEIVED, - QSP_STATE_PACKET_ID_RECEIVED, QSP_STATE_PAYLOAD_RECEIVED, QSP_STATE_CRC_RECEIVED }; enum deviceStates { DEVICE_STATE_OK, - DEVICE_STATE_FAILSAFE + DEVICE_STATE_FAILSAFE, + DEVICE_STATE_UNDETERMINED }; enum debugConfigFlags { @@ -60,6 +60,8 @@ enum debugConfigFlags { #define PPM_INPUT_CHANNEL_COUNT 10 #define PPM_OUTPUT_CHANNEL_COUNT 10 +#define TX_BUZZER_PIN A5 + #define PPM_CHANNEL_DEFAULT_VALUE 1500 //set the default servo value #define PPM_FRAME_LENGTH 30500 //set the PPM frame length in microseconds (1ms = 1000µs) #define PPM_PULSE_LENGTH 300 //set the pulse length @@ -74,20 +76,31 @@ struct QspConfiguration_t { uint8_t payloadLength = 0; uint8_t frameToSend = 0; uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0}; - uint32_t lastFrameTransmitedAt[QSP_FRAME_COUNT] = {0}; - uint8_t deviceState = DEVICE_STATE_OK; + uint32_t anyFrameRecivedAt = 0; + uint8_t deviceState = DEVICE_STATE_UNDETERMINED; void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*); - uint8_t lastReceivedPacketId = 0; 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 rssi = 0; + uint8_t snr = 0; + uint8_t flags = 0; + uint32_t roundtrip = 0; + bool readPacket = false; + bool isReceiving = false; //Indicates that TX module is receiving frames from RX module }; struct RxDeviceState_t { - int rssi = 0; - float snr = 0; + uint8_t rssi = 0; + uint8_t snr = 0; uint8_t rxVoltage = 0; uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; + uint8_t flags = 0; }; \ No newline at end of file