From 8f194e3968ff9eb2999c435596f36e77b6be95d3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Oct 2017 20:21:13 +0200 Subject: [PATCH 01/26] close #18 --- crossbow.ino | 9 +++++---- variables.h | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index d5a2926..ffbf8d9 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,9 +1,9 @@ #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX -// #define DEBUG_SERIAL +#define DEBUG_SERIAL // #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -255,7 +255,8 @@ void loop(void) if ( currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] > TX_RC_FRAME_RATE && !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE + qsp.protocolState == QSP_STATE_IDLE && + ppmReader.isReceiving() ) { qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] = currentMillis; diff --git a/variables.h b/variables.h index 94797cc..80e1ee5 100644 --- a/variables.h +++ b/variables.h @@ -9,8 +9,8 @@ #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_RX_HEALTH_FRAME_RATE 500 +#define TX_RC_FRAME_RATE 50 //ms #define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8) #define TX_PING_RATE 2000 From 429d808fa2cc7687b0d0ceba91cf32ecae34adf5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Oct 2017 21:18:01 +0200 Subject: [PATCH 02/26] close #10 --- crossbow.ino | 73 ++++++++++++++++++++++++++++++++++++++++++++++-------------- qsp.cpp | 15 +++++-------- variables.h | 7 ++++-- 3 files changed, 66 insertions(+), 29 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index ffbf8d9..6798b3e 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -3,8 +3,10 @@ #define DEVICE_MODE_TX // #define DEVICE_MODE_RX -#define DEBUG_SERIAL -// #define DEBUG_PING_PONG +#define FEATURE_TX_OLED + +// #define DEBUG_SERIAL +#define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -25,14 +27,17 @@ 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); + +#ifdef FEATURE_TX_OLED + +#define OLED_RESET -1 +#include +Adafruit_SSD1306 display(OLED_RESET); +uint32_t lastOledTaskTime = 0; + +#endif #endif @@ -143,15 +148,6 @@ void setup(void) #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,6 +165,15 @@ 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 + #endif pinMode(LED_BUILTIN, OUTPUT); @@ -231,6 +236,40 @@ void loop(void) #ifdef DEVICE_MODE_TX +#ifdef FEATURE_TX_OLED + if ( + currentMillis - lastOledTaskTime > OLED_UPDATE_RATE + ) { + lastOledTaskTime = currentMillis; + + display.clearDisplay(); + + display.setTextColor(WHITE, BLACK); + display.setCursor(0, 0); + display.print("TX RSSI: "); + display.print(map(getRadioRssi(), 0, 255, 0, 100)); + + display.setCursor(0, 12); + display.print("TX SNR: "); + display.print(getRadioSnr()); + + display.setCursor(0, 24); + display.print("RX RSSI: "); + display.print(map(rxDeviceState.rssi, 0, 255, 0, 100)); + + display.setCursor(0, 36); + display.print("RX SNR: "); + display.print(rxDeviceState.snr); + + display.setCursor(0, 46); + display.print("Roundtrip: "); + display.print(rxDeviceState.roundtrip); + + display.display(); + + } +#endif + #ifdef DEBUG_PING_PONG //PING frame if ( diff --git a/qsp.cpp b/qsp.cpp index fde358c..54b0454 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -256,18 +256,13 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p break; case QSP_FRAME_PONG: - if (qsp->debugConfig & DEBUG_FLAG_SERIAL) { + rxDeviceState->roundtrip = qsp->payload[0]; + rxDeviceState->roundtrip += (uint32_t) qsp->payload[1] << 8; + rxDeviceState->roundtrip += (uint32_t) qsp->payload[2] << 16; + rxDeviceState->roundtrip += (uint32_t) qsp->payload[3] << 24; - uint32_t incoming = 0; + rxDeviceState->roundtrip = (micros() - rxDeviceState->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: diff --git a/variables.h b/variables.h index 80e1ee5..ab48bde 100644 --- a/variables.h +++ b/variables.h @@ -1,5 +1,7 @@ #pragma once +#define OLED_UPDATE_RATE 300 + #define SBUS_UPDATE_RATE 15 //ms #define SBUS_PACKET_LENGTH 25 @@ -9,11 +11,11 @@ #define RX_TASK_HEALTH 200 //5Hz should be enough #define RSSI_CHANNEL 11 -#define RX_RX_HEALTH_FRAME_RATE 500 +#define RX_RX_HEALTH_FRAME_RATE 503 #define TX_RC_FRAME_RATE 50 //ms #define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8) -#define TX_PING_RATE 2000 +#define TX_PING_RATE 2007 #define CHANNEL_ID 0x01 #define QSP_PREAMBLE 0x51 @@ -90,4 +92,5 @@ struct RxDeviceState_t { uint8_t rxVoltage = 0; uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; + uint32_t roundtrip = 0; }; \ No newline at end of file From 0b04e327763fd20ad13b620096845dc322a391d3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Oct 2017 21:34:53 +0200 Subject: [PATCH 03/26] LED blink on RX when receiving, Constant on FAILSAFE --- crossbow.ino | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 6798b3e..6dc6dcc 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,7 +1,7 @@ #define LORA_HARDWARE_SPI -#define DEVICE_MODE_TX -// #define DEVICE_MODE_RX +// #define DEVICE_MODE_TX +#define DEVICE_MODE_RX #define FEATURE_TX_OLED @@ -325,6 +325,13 @@ void loop(void) 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)); + } + } /* From c409214de846bedf0582bc35b4da2f7f97116c46 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Oct 2017 22:03:54 +0200 Subject: [PATCH 04/26] updated SNR scaling --- crossbow.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crossbow.ino b/crossbow.ino index 6dc6dcc..56a89bf 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -66,7 +66,7 @@ uint8_t getRadioRssi(void) float getRadioSnr(void) { - return (uint8_t) constrain(LoRa.packetSnr() * 10, 0, 255); + return (uint8_t) constrain(LoRa.packetSnr(), 0, 255); } void radioPacketStart(void) From a800031b0ad2d065c4f12fd9fdf02395d62e3e09 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 26 Oct 2017 13:51:34 +0200 Subject: [PATCH 05/26] close #15 --- .vscode/arduino.json | 2 +- README.md | 4 +--- crossbow.ino | 1 - qsp.cpp | 22 ---------------------- qsp.h | 1 - variables.h | 2 -- 6 files changed, 2 insertions(+), 30 deletions(-) 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..b2f59f2 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 @@ -48,7 +47,6 @@ 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 | ### `PING` and `PONG` frames diff --git a/crossbow.ino b/crossbow.ino index 56a89bf..ea48a69 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -11,7 +11,6 @@ // #define WAIT_FOR_SERIAL #include -// #include #include "variables.h" #include "sbus.h" #include "qsp.h" diff --git a/qsp.cpp b/qsp.cpp index 54b0454..07a0978 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -60,7 +60,6 @@ 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; qsp->payloadLength = 6; } @@ -71,7 +70,6 @@ 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 } /** @@ -127,13 +125,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++) @@ -148,7 +139,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p 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) { @@ -169,7 +159,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p qspClearPayload(qsp); receivedPayload = 0; - packetId = 0; } else { @@ -188,12 +177,6 @@ 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) - { //Now it's time for payload qsp->crc ^= incomingByte; @@ -221,8 +204,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p qsp->lastFrameReceivedAt[frameId] = millis(); } - qsp->lastReceivedPacketId = packetId; - if (qsp->debugConfig & DEBUG_FLAG_SERIAL) { Serial.print("Frame "); Serial.print(frameId); @@ -299,9 +280,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..92abe68 100644 --- a/qsp.h +++ b/qsp.h @@ -9,7 +9,6 @@ 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 qspClearPayload(QspConfiguration_t *qsp); void qspEncodeFrame(QspConfiguration_t *qsp); diff --git a/variables.h b/variables.h index ab48bde..4769f95 100644 --- a/variables.h +++ b/variables.h @@ -41,7 +41,6 @@ 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 }; @@ -79,7 +78,6 @@ struct QspConfiguration_t { uint32_t lastFrameTransmitedAt[QSP_FRAME_COUNT] = {0}; uint8_t deviceState = DEVICE_STATE_OK; void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*); - uint8_t lastReceivedPacketId = 0; bool canTransmit = false; bool forcePongFrame = false; uint8_t debugConfig = 0; From 9e0f425ee9896d9f589e1fb484673d3c5f06b3a9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 26 Oct 2017 16:16:21 +0200 Subject: [PATCH 06/26] Sloted approach on TX side --- crossbow.ino | 249 +++++++++++++++++++++++++++++++++++------------------------ variables.h | 6 +- 2 files changed, 149 insertions(+), 106 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index ea48a69..162f232 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,12 +1,12 @@ #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX #define FEATURE_TX_OLED -// #define DEBUG_SERIAL -#define DEBUG_PING_PONG +#define DEBUG_SERIAL +// #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -91,14 +91,6 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp) #endif -/* -display.clearDisplay(); -display.setCursor(0,0); -display.print("Lat:"); -display.print(remoteData.latitude); -display.display(); -*/ - void setup(void) { #ifdef DEBUG_SERIAL @@ -193,6 +185,47 @@ 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, + -1, + -1, + -1, + -1, + QSP_FRAME_RX_HEALTH, + -1, + -1, + -1, + -1, + -1, + -1, + -1 +}; + +uint8_t currentSequenceIndex = 0; +#define TRANSMIT_SEQUENCE_COUNT 16 + #ifdef DEVICE_MODE_RX void updateRxDeviceState(RxDeviceState_t *rxDeviceState) { @@ -203,6 +236,30 @@ void updateRxDeviceState(RxDeviceState_t *rxDeviceState) { rxDeviceState->snr = getRadioSnr(); } +int8_t getFrameToTransmit() { + int8_t retVal = rxSendSequence[currentSequenceIndex]; + + currentSequenceIndex++; + if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) { + currentSequenceIndex = 0; + } + + return retVal; +} + +#endif + +#ifdef DEVICE_MODE_TX +int8_t getFrameToTransmit() { + int8_t retVal = txSendSequence[currentSequenceIndex]; + + currentSequenceIndex++; + if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) { + currentSequenceIndex = 0; + } + + return retVal; +} #endif void loop(void) @@ -220,18 +277,53 @@ void loop(void) qsp.protocolState = QSP_STATE_IDLE; } +#ifdef DEVICE_MODE_TX + if ( - qsp.forcePongFrame && - !transmitPayload && + abs(currentMillis - qsp.lastTxSlotTimestamp) > TX_TRANSMIT_SLOT_RATE && qsp.protocolState == QSP_STATE_IDLE - ) - { - qsp.forcePongFrame = false; - qsp.lastFrameTransmitedAt[QSP_FRAME_PONG] = currentMillis; - qsp.frameToSend = QSP_FRAME_PONG; - transmitPayload = true; + ) { + + int8_t frameToSend = getFrameToTransmit(); + + if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) { + frameToSend = -1; + } + + if (frameToSend > -1) { + + 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 + + // if ( + // qsp.forcePongFrame && + // !transmitPayload && + // qsp.protocolState == QSP_STATE_IDLE + // ) + // { + // qsp.forcePongFrame = false; + // qsp.frameToSend = QSP_FRAME_PONG; + // transmitPayload = true; + // } + #ifdef DEVICE_MODE_TX @@ -269,104 +361,55 @@ void loop(void) } #endif -#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; - - transmitPayload = true; - } #endif - /* - * RC_DATA QSP frame - */ - if ( - currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] > TX_RC_FRAME_RATE && - !transmitPayload && - qsp.protocolState == QSP_STATE_IDLE && - ppmReader.isReceiving() - ) - { - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] = currentMillis; +// #ifdef DEVICE_MODE_RX - qspClearPayload(&qsp); - encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT); - qsp.frameToSend = QSP_FRAME_RC_DATA; +// if (currentMillis > sbusTime) { +// sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); +// Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); - transmitPayload = true; - } +// sbusTime = currentMillis + SBUS_UPDATE_RATE; +// } -#endif +// /* +// * This routine updates RX device state and updates one of radio channels with RSSI value +// */ +// if (currentMillis - lastRxStateTaskTime > RX_TASK_HEALTH) { +// lastRxStateTaskTime = currentMillis; +// updateRxDeviceState(&rxDeviceState); +// ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 255, 1000, 2000); -#ifdef DEVICE_MODE_RX +// if (qsp.deviceState == DEVICE_STATE_FAILSAFE) { +// digitalWrite(LED_BUILTIN, HIGH); +// } else { +// digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); +// } - if (currentMillis > sbusTime) { - sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); - Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); +// } - sbusTime = currentMillis + SBUS_UPDATE_RATE; - } +// /* +// * RX_HEALTH QSP frame +// */ +// if ( +// currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] > RX_RX_HEALTH_FRAME_RATE && +// !transmitPayload && +// qsp.protocolState == QSP_STATE_IDLE +// ) +// { +// qspClearPayload(&qsp); +// encodeRxHealthPayload(&qsp, &rxDeviceState); +// qsp.frameToSend = QSP_FRAME_RX_HEALTH; - /* - * This routine updates RX device state and updates one of radio channels with RSSI value - */ - if (currentMillis - lastRxStateTaskTime > RX_TASK_HEALTH) { - 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 - */ - 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; - - transmitPayload = true; - } - -#endif +// transmitPayload = true; +// } +// #endif if (qsp.canTransmit && transmitPayload) { 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; } diff --git a/variables.h b/variables.h index 4769f95..d7e80d2 100644 --- a/variables.h +++ b/variables.h @@ -12,8 +12,8 @@ #define RSSI_CHANNEL 11 #define RX_RX_HEALTH_FRAME_RATE 503 -#define TX_RC_FRAME_RATE 50 //ms -#define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8) +#define TX_TRANSMIT_SLOT_RATE 57 //ms +#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8) #define TX_PING_RATE 2007 @@ -75,13 +75,13 @@ 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; void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*); bool canTransmit = false; bool forcePongFrame = false; uint8_t debugConfig = 0; uint32_t frameDecodingStartedAt = 0; + uint32_t lastTxSlotTimestamp = 0; }; struct RxDeviceState_t { From 9ea446de79cf13cc29ad543236b2146a55dd0dc1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 26 Oct 2017 20:03:20 +0200 Subject: [PATCH 07/26] Slots for transmitting on RX side close #8 --- README.md | 4 +- crossbow.ino | 151 +++++++++++++++++++++++++++++++---------------------------- qsp.cpp | 21 +-------- variables.h | 4 +- 4 files changed, 84 insertions(+), 96 deletions(-) diff --git a/README.md b/README.md index b2f59f2..1f6d2c5 100644 --- a/README.md +++ b/README.md @@ -25,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 diff --git a/crossbow.ino b/crossbow.ino index 162f232..8653739 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,11 +1,11 @@ #define LORA_HARDWARE_SPI -#define DEVICE_MODE_TX -// #define DEVICE_MODE_RX +// #define DEVICE_MODE_TX +#define DEVICE_MODE_RX #define FEATURE_TX_OLED -#define DEBUG_SERIAL +// #define DEBUG_SERIAL // #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -63,7 +63,7 @@ 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(), 0, 255); } @@ -209,7 +209,7 @@ int8_t rxSendSequence[16] = { -1, -1, -1, - -1, + QSP_FRAME_RX_HEALTH, -1, -1, -1, @@ -217,7 +217,7 @@ int8_t rxSendSequence[16] = { -1, -1, -1, - -1, + QSP_FRAME_RX_HEALTH, -1, -1, -1 @@ -236,9 +236,15 @@ void updateRxDeviceState(RxDeviceState_t *rxDeviceState) { rxDeviceState->snr = getRadioSnr(); } -int8_t getFrameToTransmit() { - int8_t retVal = rxSendSequence[currentSequenceIndex]; +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; @@ -250,7 +256,7 @@ int8_t getFrameToTransmit() { #endif #ifdef DEVICE_MODE_TX -int8_t getFrameToTransmit() { +int8_t getFrameToTransmit(QspConfiguration_t *qsp) { int8_t retVal = txSendSequence[currentSequenceIndex]; currentSequenceIndex++; @@ -284,7 +290,7 @@ void loop(void) qsp.protocolState == QSP_STATE_IDLE ) { - int8_t frameToSend = getFrameToTransmit(); + int8_t frameToSend = getFrameToTransmit(&qsp); if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) { frameToSend = -1; @@ -313,17 +319,70 @@ void loop(void) #endif - // if ( - // qsp.forcePongFrame && - // !transmitPayload && - // qsp.protocolState == QSP_STATE_IDLE - // ) - // { - // qsp.forcePongFrame = false; - // qsp.frameToSend = QSP_FRAME_PONG; - // transmitPayload = true; - // } +#ifdef DEVICE_MODE_RX + /* + * This routine updates RX device state and updates one of radio channels with RSSI value + */ + if (abs(currentMillis - lastRxStateTaskTime) > RX_TASK_HEALTH) { + 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)); + } + } + + /* + * Main routine to answer to TX module + */ + 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; + } + + } + + if (currentMillis > sbusTime) { + sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); + Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); + sbusTime = currentMillis + SBUS_UPDATE_RATE; + } + + 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 DEVICE_MODE_TX @@ -363,48 +422,6 @@ void loop(void) #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) { -// 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 -// */ -// if ( -// currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] > RX_RX_HEALTH_FRAME_RATE && -// !transmitPayload && -// qsp.protocolState == QSP_STATE_IDLE -// ) -// { -// qspClearPayload(&qsp); -// encodeRxHealthPayload(&qsp, &rxDeviceState); -// qsp.frameToSend = QSP_FRAME_RX_HEALTH; - -// transmitPayload = true; -// } -// #endif - if (qsp.canTransmit && transmitPayload) { radioPacketStart(); @@ -413,16 +430,6 @@ void loop(void) 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 diff --git a/qsp.cpp b/qsp.cpp index 07a0978..98d961c 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -204,19 +204,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p qsp->lastFrameReceivedAt[frameId] = millis(); } - 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); @@ -224,12 +211,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: @@ -251,6 +232,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p //TODO do something in this case break; } + + qsp->transmitWindowOpen = true; } else { diff --git a/variables.h b/variables.h index d7e80d2..126194d 100644 --- a/variables.h +++ b/variables.h @@ -11,12 +11,9 @@ #define RX_TASK_HEALTH 200 //5Hz should be enough #define RSSI_CHANNEL 11 -#define RX_RX_HEALTH_FRAME_RATE 503 #define TX_TRANSMIT_SLOT_RATE 57 //ms #define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8) -#define TX_PING_RATE 2007 - #define CHANNEL_ID 0x01 #define QSP_PREAMBLE 0x51 #define QSP_PAYLOAD_LENGTH 32 @@ -82,6 +79,7 @@ struct QspConfiguration_t { uint8_t debugConfig = 0; uint32_t frameDecodingStartedAt = 0; uint32_t lastTxSlotTimestamp = 0; + bool transmitWindowOpen = false; }; struct RxDeviceState_t { From b50564f4bd503ed348dc9d9508b51e7c8812a4fa Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 26 Oct 2017 20:18:02 +0200 Subject: [PATCH 08/26] Default bandwidth increased to 500kHz --- crossbow.ino | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 8653739..fa49c1d 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,7 +1,7 @@ #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX #define FEATURE_TX_OLED @@ -130,7 +130,7 @@ void setup(void) while (true); } - LoRa.setSignalBandwidth(250E3); + LoRa.setSignalBandwidth(500E3); LoRa.setSpreadingFactor(7); LoRa.setCodingRate4(5); From 86f70accff49b1553a1ed28725416a2c9f0e1c9e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 07:20:43 +0200 Subject: [PATCH 09/26] TX module is no longer using interrupts to read from LoRa. This fixes jumpy PPM readouts --- crossbow.ino | 38 ++++++++++++++++++++++++++++---------- qsp.cpp | 5 +++-- variables.h | 6 +++--- 3 files changed, 34 insertions(+), 15 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index fa49c1d..68454f4 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,11 +1,11 @@ #define LORA_HARDWARE_SPI -#define DEVICE_MODE_TX -// #define DEVICE_MODE_RX +// #define DEVICE_MODE_TX +#define DEVICE_MODE_RX #define FEATURE_TX_OLED -// #define DEBUG_SERIAL +#define DEBUG_SERIAL // #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -20,7 +20,7 @@ #define LORA32U4_RST_PIN 4 #define LORA32U4_DI0_PIN 7 -int ppm[16] = {0}; +volatile int ppm[16] = {0}; /* * Main defines for device working in TX mode @@ -52,8 +52,8 @@ uint32_t lastOledTaskTime = 0; /* * Start of QSP protocol implementation */ -QspConfiguration_t qsp = {}; -RxDeviceState_t rxDeviceState = {}; +volatile QspConfiguration_t qsp = {}; +volatile RxDeviceState_t rxDeviceState = {}; #ifdef LORA_HARDWARE_SPI @@ -134,7 +134,13 @@ void setup(void) LoRa.setSpreadingFactor(7); LoRa.setCodingRate4(5); + /* + * Use interrupt driven approach only on RX side + * TX interrupts breaks PPM readout + */ +#ifdef DEVICE_MODE_RX LoRa.onReceive(onReceive); +#endif LoRa.receive(); #endif @@ -270,9 +276,23 @@ int8_t getFrameToTransmit(QspConfiguration_t *qsp) { void loop(void) { +#ifdef DEVICE_MODE_TX + if (LoRa.available()) { + qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + } +#endif + uint32_t currentMillis = millis(); bool transmitPayload = false; +#ifdef DEBUG_SERIAL + static uint32_t r = 0; + if (currentMillis - r > 1000) { + Serial.println(currentMillis / 1000); + r = currentMillis; + } +#endif + /* * Watchdog for frame decoding stuck somewhere in the middle of a process */ @@ -335,7 +355,7 @@ void loop(void) digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); } } - + /* * Main routine to answer to TX module */ @@ -432,7 +452,6 @@ void loop(void) } -#ifdef LORA_HARDWARE_SPI void onReceive(int packetSize) { if (packetSize == 0) @@ -442,5 +461,4 @@ void onReceive(int packetSize) { qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); } -} -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/qsp.cpp b/qsp.cpp index 98d961c..5fb14e1 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -142,7 +142,6 @@ 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; @@ -177,6 +176,9 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p } else if (qsp->protocolState == QSP_STATE_FRAME_TYPE_RECEIVED) { + if (receivedPayload >= QSP_PAYLOAD_LENGTH) { + qsp->protocolState = QSP_STATE_IDLE; + } //Now it's time for payload qsp->crc ^= incomingByte; @@ -192,7 +194,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 diff --git a/variables.h b/variables.h index 126194d..174776d 100644 --- a/variables.h +++ b/variables.h @@ -18,7 +18,7 @@ #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 @@ -83,8 +83,8 @@ struct QspConfiguration_t { }; 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; From 5c090b68684a4cc297d7cad757a8b15597aa854c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 09:56:46 +0200 Subject: [PATCH 10/26] Fixed race condition that casued concurrent SPI operation and hunged the RX module --- crossbow.ino | 20 ++++---------------- track1.txt | 31 +++++++++++++++++++++++++++++++ track2.txt | 14 ++++++++++++++ track3.txt | 10 ++++++++++ 4 files changed, 59 insertions(+), 16 deletions(-) create mode 100644 track1.txt create mode 100644 track2.txt create mode 100644 track3.txt diff --git a/crossbow.ino b/crossbow.ino index 68454f4..de56112 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -5,7 +5,7 @@ #define FEATURE_TX_OLED -#define DEBUG_SERIAL +// #define DEBUG_SERIAL // #define DEBUG_PING_PONG // #define DEBUG_LED // #define WAIT_FOR_SERIAL @@ -238,8 +238,6 @@ 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) { @@ -285,14 +283,6 @@ void loop(void) uint32_t currentMillis = millis(); bool transmitPayload = false; -#ifdef DEBUG_SERIAL - static uint32_t r = 0; - if (currentMillis - r > 1000) { - Serial.println(currentMillis / 1000); - r = currentMillis; - } -#endif - /* * Watchdog for frame decoding stuck somewhere in the middle of a process */ @@ -348,7 +338,6 @@ void loop(void) 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 { @@ -360,19 +349,15 @@ void loop(void) * Main routine to answer to TX module */ 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: /* @@ -461,4 +446,7 @@ void onReceive(int packetSize) { qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); } + + rxDeviceState.rssi = getRadioRssi(); + rxDeviceState.snr = getRadioSnr(); } \ No newline at end of file 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 From 0f735f005477d3ef51e75cdd15e0a57e1c074475 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 11:25:48 +0200 Subject: [PATCH 11/26] close #23 --- README.md | 8 ++++++++ crossbow.ino | 4 ++-- qsp.cpp | 9 +++++++++ variables.h | 1 + 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1f6d2c5..28d5bab 100644 --- a/README.md +++ b/README.md @@ -47,6 +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 | Flags | + +#### Flags + +| Bit | Meaning | +| ---- | ---- | +| 00000001 | Device in Failsafe mode | + ### `PING` and `PONG` frames diff --git a/crossbow.ino b/crossbow.ino index de56112..bf29680 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,7 +1,7 @@ #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX #define FEATURE_TX_OLED diff --git a/qsp.cpp b/qsp.cpp index 5fb14e1..8f37fae 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -61,6 +61,14 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta qsp->payload[3] = rxDeviceState->a1Voltage; qsp->payload[4] = rxDeviceState->a2Voltage; + uint8_t flags = 0; + + if (qsp->deviceState == DEVICE_STATE_FAILSAFE) { + flags |= 0x01 << 0; + } + + qsp->payload[5] = flags; + qsp->payloadLength = 6; } @@ -70,6 +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->flags = qsp->payload[5]; } /** diff --git a/variables.h b/variables.h index 174776d..993fc67 100644 --- a/variables.h +++ b/variables.h @@ -89,4 +89,5 @@ struct RxDeviceState_t { uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; uint32_t roundtrip = 0; + uint8_t flags = 0; }; \ No newline at end of file From 85b1dbe26d3a0c3e7e5b6595b96461201f6e458a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 13:18:22 +0200 Subject: [PATCH 12/26] performance improvements --- crossbow.ino | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index bf29680..c56498e 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -138,9 +138,9 @@ void setup(void) * Use interrupt driven approach only on RX side * TX interrupts breaks PPM readout */ -#ifdef DEVICE_MODE_RX +// #ifdef DEVICE_MODE_RX LoRa.onReceive(onReceive); -#endif +// #endif LoRa.receive(); #endif @@ -272,11 +272,20 @@ int8_t getFrameToTransmit(QspConfiguration_t *qsp) { } #endif +volatile bool readPacket = false; +uint8_t myRssi = 0; +uint8_t mySnr = 0; + void loop(void) { #ifdef DEVICE_MODE_TX - if (LoRa.available()) { - qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + if (readPacket) { + while (LoRa.available()) { + qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + } + myRssi = getRadioRssi(); + mySnr = getRadioSnr(); + readPacket = false; } #endif @@ -402,11 +411,11 @@ void loop(void) display.setTextColor(WHITE, BLACK); display.setCursor(0, 0); display.print("TX RSSI: "); - display.print(map(getRadioRssi(), 0, 255, 0, 100)); + display.print(map(myRssi, 0, 255, 0, 100)); display.setCursor(0, 12); display.print("TX SNR: "); - display.print(getRadioSnr()); + display.print(mySnr); display.setCursor(0, 24); display.print("RX RSSI: "); @@ -442,6 +451,11 @@ void onReceive(int packetSize) if (packetSize == 0) return; +#ifdef DEVICE_MODE_TX + readPacket = true; +#endif + +#ifdef DEVICE_MODE_RX while (LoRa.available()) { qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); @@ -449,4 +463,5 @@ void onReceive(int packetSize) rxDeviceState.rssi = getRadioRssi(); rxDeviceState.snr = getRadioSnr(); +#endif } \ No newline at end of file From 6277d4cec912df151bcab9264c83a5b4ede22b8c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 13:44:01 +0200 Subject: [PATCH 13/26] Code cleanup --- crossbow.ino | 23 ++++++++++------------- variables.h | 10 +++++++++- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index c56498e..f5bbad7 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,7 +1,7 @@ #define LORA_HARDWARE_SPI -#define DEVICE_MODE_TX -// #define DEVICE_MODE_RX +// #define DEVICE_MODE_TX +#define DEVICE_MODE_RX #define FEATURE_TX_OLED @@ -54,6 +54,7 @@ uint32_t lastOledTaskTime = 0; */ volatile QspConfiguration_t qsp = {}; volatile RxDeviceState_t rxDeviceState = {}; +volatile TxDeviceState_t txDeviceState = {}; #ifdef LORA_HARDWARE_SPI @@ -272,20 +273,16 @@ int8_t getFrameToTransmit(QspConfiguration_t *qsp) { } #endif -volatile bool readPacket = false; -uint8_t myRssi = 0; -uint8_t mySnr = 0; - void loop(void) { #ifdef DEVICE_MODE_TX - if (readPacket) { + if (txDeviceState.readPacket) { while (LoRa.available()) { qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); } - myRssi = getRadioRssi(); - mySnr = getRadioSnr(); - readPacket = false; + txDeviceState.rssi = getRadioRssi(); + txDeviceState.snr = getRadioSnr(); + txDeviceState.readPacket = false; } #endif @@ -411,11 +408,11 @@ void loop(void) display.setTextColor(WHITE, BLACK); display.setCursor(0, 0); display.print("TX RSSI: "); - display.print(map(myRssi, 0, 255, 0, 100)); + display.print(map(txDeviceState.rssi, 0, 255, 0, 100)); display.setCursor(0, 12); display.print("TX SNR: "); - display.print(mySnr); + display.print(txDeviceState.snr); display.setCursor(0, 24); display.print("RX RSSI: "); @@ -452,7 +449,7 @@ void onReceive(int packetSize) return; #ifdef DEVICE_MODE_TX - readPacket = true; + txDeviceState.readPacket = true; #endif #ifdef DEVICE_MODE_RX diff --git a/variables.h b/variables.h index 993fc67..c1f8b62 100644 --- a/variables.h +++ b/variables.h @@ -11,7 +11,7 @@ #define RX_TASK_HEALTH 200 //5Hz should be enough #define RSSI_CHANNEL 11 -#define TX_TRANSMIT_SLOT_RATE 57 //ms +#define TX_TRANSMIT_SLOT_RATE 50 //ms #define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8) #define CHANNEL_ID 0x01 @@ -82,6 +82,14 @@ struct QspConfiguration_t { 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; +}; + struct RxDeviceState_t { uint8_t rssi = 0; uint8_t snr = 0; From 9703d3144fa7c4b3648829da4289295ea94e73fe Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Oct 2017 13:56:12 +0200 Subject: [PATCH 14/26] Another small optimization to limit resource hogging --- crossbow.ino | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index f5bbad7..b961801 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,7 +1,7 @@ #define LORA_HARDWARE_SPI -// #define DEVICE_MODE_TX -#define DEVICE_MODE_RX +#define DEVICE_MODE_TX +// #define DEVICE_MODE_RX #define FEATURE_TX_OLED @@ -277,12 +277,13 @@ void loop(void) { #ifdef DEVICE_MODE_TX if (txDeviceState.readPacket) { - while (LoRa.available()) { + if (LoRa.available()) { qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + } else { + txDeviceState.rssi = getRadioRssi(); + txDeviceState.snr = getRadioSnr(); + txDeviceState.readPacket = false; } - txDeviceState.rssi = getRadioRssi(); - txDeviceState.snr = getRadioSnr(); - txDeviceState.readPacket = false; } #endif From d29f76fa7ccc6f907edc674017ce9734f21bc537 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Oct 2017 16:55:48 +0200 Subject: [PATCH 15/26] buzzer processing code --- buzzer.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ buzzer.h | 43 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 buzzer.c create mode 100644 buzzer.h diff --git a/buzzer.c b/buzzer.c new file mode 100644 index 0000000..59c28b9 --- /dev/null +++ b/buzzer.c @@ -0,0 +1,48 @@ +#include "Arduino.h" +#include "buzzer.h" + +void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) +{ + + if (!buzzer->enabled) + { + 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->updateTime = timestamp + buzzer->patternTickPerdiod; + } +}; \ No newline at end of file diff --git a/buzzer.h b/buzzer.h new file mode 100644 index 0000000..ec09b37 --- /dev/null +++ b/buzzer.h @@ -0,0 +1,43 @@ +#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 = true; + uint8_t mode = BUZZER_MODE_DOUBLE_CHIRP; + + uint32_t updateTime = 0; + + uint8_t tick = 0; + uint8_t element = 0; + const uint8_t patternMaxTick = 20; + const uint8_t patternTickPerdiod = 100; + + 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, 10, 11}, + {0, 1, 2, 3} + }; + +}; + +void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); \ No newline at end of file From 74cb08401733a029522087ed59cdf121e949066a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Oct 2017 19:35:06 +0200 Subject: [PATCH 16/26] Basic buzzer framework --- crossbow.ino | 24 +++++++++++++----------- buzzer.c => txbuzzer.cpp | 2 +- buzzer.h => txbuzzer.h | 8 ++++---- variables.h | 4 +++- 4 files changed, 21 insertions(+), 17 deletions(-) rename buzzer.c => txbuzzer.cpp (97%) rename buzzer.h => txbuzzer.h (82%) diff --git a/crossbow.ino b/crossbow.ino index b961801..ab4472a 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -1,5 +1,3 @@ -#define LORA_HARDWARE_SPI - #define DEVICE_MODE_TX // #define DEVICE_MODE_RX @@ -12,16 +10,15 @@ #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 -volatile int ppm[16] = {0}; - /* * Main defines for device working in TX mode */ @@ -29,6 +26,10 @@ volatile int ppm[16] = {0}; #include PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true); +#include "txbuzzer.h" + +BuzzerState_t buzzer; + #ifdef FEATURE_TX_OLED #define OLED_RESET -1 @@ -44,6 +45,9 @@ uint32_t lastOledTaskTime = 0; * 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; @@ -56,7 +60,6 @@ volatile QspConfiguration_t qsp = {}; volatile RxDeviceState_t rxDeviceState = {}; volatile TxDeviceState_t txDeviceState = {}; -#ifdef LORA_HARDWARE_SPI uint8_t getRadioRssi(void) { @@ -90,8 +93,6 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp) LoRa.write(dataByte); } -#endif - void setup(void) { #ifdef DEBUG_SERIAL @@ -106,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 @@ -143,7 +142,6 @@ void setup(void) LoRa.onReceive(onReceive); // #endif LoRa.receive(); -#endif #ifdef DEVICE_MODE_RX //initiallize default ppm values @@ -164,6 +162,8 @@ void setup(void) TCCR1B = 0; TCCR1B |= (1 << CS11); //set timer1 to increment every 0,5 us or 1us on 8MHz + pinMode(TX_BUZZER_PIN, OUTPUT); + #ifdef FEATURE_TX_OLED display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) display.setTextSize(1); @@ -398,6 +398,8 @@ void loop(void) #ifdef DEVICE_MODE_TX + buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer); + #ifdef FEATURE_TX_OLED if ( currentMillis - lastOledTaskTime > OLED_UPDATE_RATE diff --git a/buzzer.c b/txbuzzer.cpp similarity index 97% rename from buzzer.c rename to txbuzzer.cpp index 59c28b9..4774ecf 100644 --- a/buzzer.c +++ b/txbuzzer.cpp @@ -1,5 +1,5 @@ #include "Arduino.h" -#include "buzzer.h" +#include "txbuzzer.h" void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { diff --git a/buzzer.h b/txbuzzer.h similarity index 82% rename from buzzer.h rename to txbuzzer.h index ec09b37..f57d320 100644 --- a/buzzer.h +++ b/txbuzzer.h @@ -19,22 +19,22 @@ enum { }; struct BuzzerState_t { - bool enabled = true; - uint8_t mode = BUZZER_MODE_DOUBLE_CHIRP; + bool enabled = 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 = 100; + 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, 10, 11}, + {0, 1, PATTERN_CYCLE_IGNORE, PATTERN_CYCLE_IGNORE}, {0, 1, 2, 3} }; diff --git a/variables.h b/variables.h index c1f8b62..a189862 100644 --- a/variables.h +++ b/variables.h @@ -1,6 +1,6 @@ #pragma once -#define OLED_UPDATE_RATE 300 +#define OLED_UPDATE_RATE 500 #define SBUS_UPDATE_RATE 15 //ms #define SBUS_PACKET_LENGTH 25 @@ -58,6 +58,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 From 7bb8544880b318120cb7fe916b33c449178f5288 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Oct 2017 20:30:56 +0200 Subject: [PATCH 17/26] optimizations for OLED --- crossbow.ino | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index ab4472a..07e3bf3 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -410,27 +410,26 @@ void loop(void) display.setTextColor(WHITE, BLACK); display.setCursor(0, 0); - display.print("TX RSSI: "); - display.print(map(txDeviceState.rssi, 0, 255, 0, 100)); + display.setTextSize(3); + display.print(txDeviceState.rssi); - display.setCursor(0, 12); - display.print("TX SNR: "); - display.print(txDeviceState.snr); + display.setCursor(18, 28); + display.setTextSize(2); + display.print(txDeviceState.snr); - display.setCursor(0, 24); - display.print("RX RSSI: "); - display.print(map(rxDeviceState.rssi, 0, 255, 0, 100)); + display.setCursor(74, 0); + display.setTextSize(3); + display.print(rxDeviceState.rssi); - display.setCursor(0, 36); - display.print("RX SNR: "); + display.setCursor(92, 28); + display.setTextSize(2); display.print(rxDeviceState.snr); - display.setCursor(0, 46); - display.print("Roundtrip: "); + display.setCursor(54, 48); + display.setTextSize(2); display.print(rxDeviceState.roundtrip); display.display(); - } #endif From 72cb29eca26cc7616445b0fe873dac0e80232279 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Oct 2017 22:25:38 +0200 Subject: [PATCH 18/26] Speed optimization for S.Bus encoding --- crossbow.ino | 4 ++-- sbus.cpp | 11 ++++++++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 07e3bf3..f3a6686 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 diff --git a/sbus.cpp b/sbus.cpp index 53ffe0c..b31eb5c 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] = mapsChannelToSbus(channels[i]); } uint8_t stateByte = 0x00; From b3cac834e2ec57e68fc573caff4512627ace425c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Oct 2017 23:07:14 +0200 Subject: [PATCH 19/26] Reduced number of SPI operations when reading RF packets --- crossbow.ino | 12 ++++++++---- sbus.cpp | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index f3a6686..afe2b32 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -277,8 +277,9 @@ void loop(void) { #ifdef DEVICE_MODE_TX if (txDeviceState.readPacket) { - if (LoRa.available()) { - qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + int incomingByte = LoRa.read(); + if (incomingByte > -1) { + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState); } else { txDeviceState.rssi = getRadioRssi(); txDeviceState.snr = getRadioSnr(); @@ -455,9 +456,12 @@ void onReceive(int packetSize) #endif #ifdef DEVICE_MODE_RX - while (LoRa.available()) + + int incomingByte; + + while (incomingByte = LoRa.read(), incomingByte > -1) { - qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState); + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState); } rxDeviceState.rssi = getRadioRssi(); diff --git a/sbus.cpp b/sbus.cpp index b31eb5c..f8ebff6 100644 --- a/sbus.cpp +++ b/sbus.cpp @@ -30,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] = mapsChannelToSbus(channels[i]); + output[i] = mapChannelToSbus(channels[i]); } uint8_t stateByte = 0x00; From a2fb5abffaf3686b0bc39a377099417ba2f56176 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Oct 2017 09:35:00 +0100 Subject: [PATCH 20/26] Buzzer Single and Continous modes --- crossbow.ino | 22 +++++++++++----------- txbuzzer.cpp | 21 ++++++++++++++++++++- txbuzzer.h | 5 ++++- 3 files changed, 35 insertions(+), 13 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index afe2b32..e5adc07 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 @@ -162,8 +162,6 @@ void setup(void) TCCR1B = 0; TCCR1B |= (1 << CS11); //set timer1 to increment every 0,5 us or 1us on 8MHz - pinMode(TX_BUZZER_PIN, OUTPUT); - #ifdef FEATURE_TX_OLED display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) display.setTextSize(1); @@ -172,17 +170,19 @@ void setup(void) 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, TX_BUZZER_PIN, millis(), &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 diff --git a/txbuzzer.cpp b/txbuzzer.cpp index 4774ecf..b01473d 100644 --- a/txbuzzer.cpp +++ b/txbuzzer.cpp @@ -1,10 +1,28 @@ #include "Arduino.h" #include "txbuzzer.h" +/** + * This method plays selected pattern only once + * It disables continious mode + */ +void buzzerSingleMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { + buzzer->singleModeEnabled = true; + buzzer->enabled = false; + buzzer->mode = mode; + buzzer->tick = 0; +} + +void buzzerContinousMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { + buzzer->singleModeEnabled = false; + buzzer->enabled = true; + buzzer->mode = mode; + buzzer->tick = 0; +} + void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { - if (!buzzer->enabled) + if (!buzzer->enabled && !buzzer->singleModeEnabled) { digitalWrite(pin, LOW); return; @@ -42,6 +60,7 @@ void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) if (buzzer->tick >= buzzer->patternMaxTick) { buzzer->tick = 0; + buzzer->singleModeEnabled = false; } buzzer->updateTime = timestamp + buzzer->patternTickPerdiod; } diff --git a/txbuzzer.h b/txbuzzer.h index f57d320..90d00ba 100644 --- a/txbuzzer.h +++ b/txbuzzer.h @@ -19,7 +19,8 @@ enum { }; struct BuzzerState_t { - bool enabled = false; + bool enabled = false; //Continous mode buzzer + bool singleModeEnabled = false; uint8_t mode = BUZZER_MODE_OFF; uint32_t updateTime = 0; @@ -40,4 +41,6 @@ struct BuzzerState_t { }; +void buzzerSingleMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); +void buzzerContinousMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); \ No newline at end of file From 86c7c3af0afa048c23545f1302b44bfe9555b2dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Oct 2017 09:56:06 +0100 Subject: [PATCH 21/26] Small optimization --- crossbow.ino | 6 +++--- qsp.cpp | 19 ++++++++++++------- qsp.h | 8 +++++++- variables.h | 1 - 4 files changed, 22 insertions(+), 12 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index e5adc07..0e0bc26 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -279,7 +279,7 @@ void loop(void) if (txDeviceState.readPacket) { int incomingByte = LoRa.read(); if (incomingByte > -1) { - qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState); + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState); } else { txDeviceState.rssi = getRadioRssi(); txDeviceState.snr = getRadioSnr(); @@ -428,7 +428,7 @@ void loop(void) display.setCursor(54, 48); display.setTextSize(2); - display.print(rxDeviceState.roundtrip); + display.print(txDeviceState.roundtrip); display.display(); } @@ -461,7 +461,7 @@ void onReceive(int packetSize) while (incomingByte = LoRa.read(), incomingByte > -1) { - qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState); + qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState); } rxDeviceState.rssi = getRadioRssi(); diff --git a/qsp.cpp b/qsp.cpp index 8f37fae..c584b60 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -143,8 +143,13 @@ 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; @@ -228,12 +233,12 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p break; case QSP_FRAME_PONG: - rxDeviceState->roundtrip = qsp->payload[0]; - rxDeviceState->roundtrip += (uint32_t) qsp->payload[1] << 8; - rxDeviceState->roundtrip += (uint32_t) qsp->payload[2] << 16; - rxDeviceState->roundtrip += (uint32_t) qsp->payload[3] << 24; + 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; - rxDeviceState->roundtrip = (micros() - rxDeviceState->roundtrip) / 1000; + txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000; break; diff --git a/qsp.h b/qsp.h index 92abe68..b801ae0 100644 --- a/qsp.h +++ b/qsp.h @@ -9,7 +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); -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/variables.h b/variables.h index a189862..6df3a2d 100644 --- a/variables.h +++ b/variables.h @@ -98,6 +98,5 @@ struct RxDeviceState_t { uint8_t rxVoltage = 0; uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; - uint32_t roundtrip = 0; uint8_t flags = 0; }; \ No newline at end of file From b7782db478879d402368b05042c03bfade4aa9f1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Oct 2017 13:08:39 +0100 Subject: [PATCH 22/26] Audible alarms on TX side --- crossbow.ino | 43 ++++++++++++++++++++++++++++++++++++++++++- qsp.cpp | 1 + txbuzzer.cpp | 5 ++--- txbuzzer.h | 4 ++-- variables.h | 8 ++++++-- 5 files changed, 53 insertions(+), 8 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 0e0bc26..e727a38 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -178,7 +178,7 @@ void setup(void) pinMode(TX_BUZZER_PIN, OUTPUT); //Play single tune to indicate power up - buzzerSingleMode(BUZZER_MODE_CHIRP, TX_BUZZER_PIN, millis(), &buzzer); + buzzerSingleMode(BUZZER_MODE_CHIRP, &buzzer); #endif pinMode(LED_BUILTIN, OUTPUT); @@ -275,6 +275,7 @@ int8_t getFrameToTransmit(QspConfiguration_t *qsp) { void loop(void) { + #ifdef DEVICE_MODE_TX if (txDeviceState.readPacket) { int incomingByte = LoRa.read(); @@ -290,6 +291,7 @@ void loop(void) uint32_t currentMillis = millis(); bool transmitPayload = false; + static uint32_t previousAnyFrameReceivedAt = 0; /* * Watchdog for frame decoding stuck somewhere in the middle of a process @@ -401,6 +403,45 @@ void loop(void) 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 && abs(currentMillis - qsp.anyFrameRecivedAt) > TX_FAILSAFE_DELAY) { + txDeviceState.isReceiving = false; + rxDeviceState.a1Voltage = 0; + rxDeviceState.a2Voltage = 0; + rxDeviceState.rxVoltage = 0; + rxDeviceState.rssi = 0; + rxDeviceState.snr = 0; + rxDeviceState.flags = 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 diff --git a/qsp.cpp b/qsp.cpp index c584b60..44d733d 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -218,6 +218,7 @@ void qspDecodeIncomingFrame( if (frameId < QSP_FRAME_COUNT) { qsp->lastFrameReceivedAt[frameId] = millis(); } + qsp->anyFrameRecivedAt = millis(); switch (frameId) { case QSP_FRAME_RC_DATA: diff --git a/txbuzzer.cpp b/txbuzzer.cpp index b01473d..5994977 100644 --- a/txbuzzer.cpp +++ b/txbuzzer.cpp @@ -5,18 +5,17 @@ * This method plays selected pattern only once * It disables continious mode */ -void buzzerSingleMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { +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, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) { +void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer) { buzzer->singleModeEnabled = false; buzzer->enabled = true; buzzer->mode = mode; - buzzer->tick = 0; } void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer) diff --git a/txbuzzer.h b/txbuzzer.h index 90d00ba..aa2bb45 100644 --- a/txbuzzer.h +++ b/txbuzzer.h @@ -41,6 +41,6 @@ struct BuzzerState_t { }; -void buzzerSingleMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); -void buzzerContinousMode(uint8_t mode, uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer); +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 6df3a2d..8840dca 100644 --- a/variables.h +++ b/variables.h @@ -13,6 +13,7 @@ #define TX_TRANSMIT_SLOT_RATE 50 //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 @@ -44,7 +45,8 @@ enum dataStates { enum deviceStates { DEVICE_STATE_OK, - DEVICE_STATE_FAILSAFE + DEVICE_STATE_FAILSAFE, + DEVICE_STATE_UNDETERMINED }; enum debugConfigFlags { @@ -74,7 +76,8 @@ struct QspConfiguration_t { uint8_t payloadLength = 0; uint8_t frameToSend = 0; uint32_t lastFrameReceivedAt[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*); bool canTransmit = false; bool forcePongFrame = false; @@ -90,6 +93,7 @@ struct TxDeviceState_t { 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 { From 34b7a42b85dabef38ab602ab075ad68b0af9e421 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Oct 2017 15:25:36 +0100 Subject: [PATCH 23/26] slower transmission --- crossbow.ino | 6 ++++-- variables.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index e727a38..244478f 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -131,8 +131,9 @@ void setup(void) } LoRa.setSignalBandwidth(500E3); - LoRa.setSpreadingFactor(7); - LoRa.setCodingRate4(5); + LoRa.setSpreadingFactor(8); + LoRa.setCodingRate4(6); + LoRa.enableCrc(); /* * Use interrupt driven approach only on RX side @@ -314,6 +315,7 @@ void loop(void) if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) { frameToSend = -1; + //FIXME uncomment to enable full Failsafe } if (frameToSend > -1) { diff --git a/variables.h b/variables.h index 8840dca..ceacb2c 100644 --- a/variables.h +++ b/variables.h @@ -11,7 +11,7 @@ #define RX_TASK_HEALTH 200 //5Hz should be enough #define RSSI_CHANNEL 11 -#define TX_TRANSMIT_SLOT_RATE 50 //ms +#define TX_TRANSMIT_SLOT_RATE 70 //ms #define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8) #define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4) From b3d8847d8634c6088c5331f16495fd72de9106dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 30 Oct 2017 16:35:48 +0100 Subject: [PATCH 24/26] OLED RRSI scaling changed to dB --- crossbow.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 244478f..b31acc1 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -455,7 +455,7 @@ void loop(void) display.setTextColor(WHITE, BLACK); display.setCursor(0, 0); display.setTextSize(3); - display.print(txDeviceState.rssi); + display.print(map(txDeviceState.rssi, 0, 255, 0, 164)); display.setCursor(18, 28); display.setTextSize(2); @@ -463,7 +463,7 @@ void loop(void) display.setCursor(74, 0); display.setTextSize(3); - display.print(rxDeviceState.rssi); + display.print(map(rxDeviceState.rssi, 0, 255, 0, 164)); display.setCursor(92, 28); display.setTextSize(2); From 3921cc245c4d03c1090a6ad53a563f2b8c9d3f45 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 30 Oct 2017 17:38:56 +0100 Subject: [PATCH 25/26] Fixed casting bug on RX side that was causing failsafe to engage without specific reason --- crossbow.ino | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index b31acc1..8b18286 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -134,14 +134,7 @@ void setup(void) LoRa.setSpreadingFactor(8); LoRa.setCodingRate4(6); LoRa.enableCrc(); - - /* - * Use interrupt driven approach only on RX side - * TX interrupts breaks PPM readout - */ -// #ifdef DEVICE_MODE_RX LoRa.onReceive(onReceive); -// #endif LoRa.receive(); #ifdef DEVICE_MODE_RX @@ -393,7 +386,7 @@ void loop(void) sbusTime = currentMillis + SBUS_UPDATE_RATE; } - if (abs(currentMillis - qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA]) > RX_FAILSAFE_DELAY) { + if (qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA] + RX_FAILSAFE_DELAY < currentMillis) { qsp.deviceState = DEVICE_STATE_FAILSAFE; } else { qsp.deviceState = DEVICE_STATE_OK; From 5ae986112f3077a85cb83e75b56e968ea0dd93ff Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 30 Oct 2017 20:39:32 +0100 Subject: [PATCH 26/26] Several small fixes for potential race conditions --- crossbow.ino | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/crossbow.ino b/crossbow.ino index 8b18286..661d7d0 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 @@ -292,7 +292,7 @@ void loop(void) */ 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; } @@ -300,8 +300,8 @@ void loop(void) #ifdef DEVICE_MODE_TX if ( - abs(currentMillis - qsp.lastTxSlotTimestamp) > TX_TRANSMIT_SLOT_RATE && - qsp.protocolState == QSP_STATE_IDLE + qsp.protocolState == QSP_STATE_IDLE && + qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis ) { int8_t frameToSend = getFrameToTransmit(&qsp); @@ -339,7 +339,7 @@ void loop(void) /* * This routine updates RX device state and updates one of radio channels with RSSI value */ - if (abs(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); @@ -408,7 +408,10 @@ void loop(void) } //Here we detect failsafe state on TX module - if (txDeviceState.isReceiving && abs(currentMillis - qsp.anyFrameRecivedAt) > TX_FAILSAFE_DELAY) { + if ( + txDeviceState.isReceiving && + qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis + ) { txDeviceState.isReceiving = false; rxDeviceState.a1Voltage = 0; rxDeviceState.a2Voltage = 0; @@ -416,6 +419,7 @@ void loop(void) rxDeviceState.rssi = 0; rxDeviceState.snr = 0; rxDeviceState.flags = 0; + txDeviceState.roundtrip = 0; qsp.deviceState = DEVICE_STATE_FAILSAFE; qsp.anyFrameRecivedAt = 0; }