From fa9e4747418e0c75d906eeecaad78af3c393682d Mon Sep 17 00:00:00 2001 From: Egor Date: Sun, 2 Mar 2025 20:24:12 -0800 Subject: [PATCH] add CSRF --- platformio.ini | 54 ++- radio/CRSF.h | 293 +++++++++++-- radio/FHSS.cpp | 8 +- radio/FHSS.h | 6 + radio/MavLink.h | 125 ++++++ radio/RadioCommands.h | 2 + radio/Readme.txt | 8 + radio/SBUS.h | 63 +++ radio/Tx.cpp | 638 +++++----------------------- radio/USB-Serial.h | 96 +++++ radio/config.h | 75 ++++ radio/i-bus.h | 161 ------- radio/iBUS.cpp | 34 -- radio/iBUS.h | 158 ++++++- radio/{s-bus.h => radio-protocol.h} | 105 +---- radio/radio.h | 378 ++++++++++++++++ radio/utility.h | 22 + 17 files changed, 1342 insertions(+), 884 deletions(-) create mode 100644 radio/MavLink.h create mode 100644 radio/SBUS.h create mode 100644 radio/USB-Serial.h create mode 100644 radio/config.h delete mode 100644 radio/i-bus.h delete mode 100644 radio/iBUS.cpp rename radio/{s-bus.h => radio-protocol.h} (61%) create mode 100644 radio/radio.h create mode 100644 radio/utility.h diff --git a/platformio.ini b/platformio.ini index ac0f17f..dbea0ee 100644 --- a/platformio.ini +++ b/platformio.ini @@ -320,6 +320,59 @@ build_flags = -DLORA_RX=1 -DLORA_TX=0 +[env:heltec-v3-900-radio-rx] +platform = espressif32 +board = heltec_wifi_lora_32_V3 +framework = arduino +upload_speed = 921600 +monitor_speed = 115200 +board_build.f_cpu = 240000000 +src_dir = radio +lib_deps = + ropg/Heltec_ESP32_LoRa_v3@^0.9.1 + RadioLib + XPowersLib + https://github.com/bolderflight/sbus + mbed-takuhachisu/CRC8 +build_flags = + -DMAIN_FILE="Tx.cpp" + -DHELTEC_POWER_BUTTON + -DHELTEC + -DRADIOLIB_CHECK_PARAMS=0 + -DESP32 + -DARDUINO_ARCH_ESP32 + -DARDUINO_USB_CDC_ON_BOOT=0 + -DARDUINO_USB_MODE=1 + -DLORA_RX=1 + -DLORA_TX=0 + +[env:heltec-v3-900-radio-tx] +platform = espressif32 +board = heltec_wifi_lora_32_V3 +framework = arduino +upload_speed = 921600 +monitor_speed = 115200 +board_build.f_cpu = 240000000 +src_dir = radio +lib_deps = + ropg/Heltec_ESP32_LoRa_v3@^0.9.1 + RadioLib + XPowersLib + https://github.com/bolderflight/sbus + mbed-takuhachisu/CRC8 +build_flags = + -DMAIN_FILE="Tx.cpp" + -DHELTEC_POWER_BUTTON + -DHELTEC + -DRADIOLIB_CHECK_PARAMS=0 + -DESP32 + -DARDUINO_ARCH_ESP32 + -DARDUINO_USB_CDC_ON_BOOT=0 + -DARDUINO_USB_MODE=1 + -DLORA_RX=0 + -DLORA_TX=1 + -DSERIAL_INPUT=1 + [env:lilygo-T3S3-v1-2-sx1262-900-radio-rx] platform = espressif32 board = t3_s3_v1_x @@ -353,7 +406,6 @@ build_flags = -DLORA_RX=1 -DLORA_TX=0 - [env:lilygo-T3S3-v1-2-lr1121-900-BT] platform = espressif32 board = t3_s3_v1_x diff --git a/radio/CRSF.h b/radio/CRSF.h index 6cff615..06f3fc7 100644 --- a/radio/CRSF.h +++ b/radio/CRSF.h @@ -1,70 +1,297 @@ +#pragma once + #include -class CRSF +// +// Standard Crossfire definitions: +// +#define CRSF_SYNC_BYTE 0xC8 +#define CRSF_FRAME_TYPE_CHANNELS 0x16 +#define CRSF_MAX_CHANNELS 16 +#define CRSF_BAUDRATE 420000 // standard CRSF baud rate + +// Betaflight CRSF docs: +// https://github.com/betaflight/betaflight/blob/4.5-maintenance/src/main/rx/crsf_protocol.h +// Crosfire Protocol specs: +// https://github.com/tbs-fpv/tbs-crsf-spec/blob/main/crsf.md#broadcast-frame + +// The CRSF "RC Channels" frame for 16 channels is always 24 bytes total: +// Byte[0] = 0xC8 (address) +// Byte[1] = 22 (length from byte[2] to the last CRC byte) +// Byte[2] = 0x16 (frame type for channels) +// Byte[3..22] = 20 bytes of channel data (10 bits × 16 = 160 bits) +// Byte[23] = CRC +// +// If type=0x16 and length=22, we interpret it as channel data (16ch, 10 bits each). + +class CRSF2 { public: - CRSF(HardwareSerial &serialPort, int txPin, int rxPin, long baudRate = 420000) - : serialPort(serialPort), txPin(txPin), rxPin(rxPin), baudRate(baudRate) + // Pass in the HardwareSerial, plus the pins to use on ESP32 + // (rxPin and txPin). If you only do TX, you can set rxPin=-1 or vice versa. + CRSF2(HardwareSerial &serialPort, int rxPin, int txPin, long baudRate = CRSF_BAUDRATE) + : serialPort(serialPort), rxPin(rxPin), txPin(txPin), baudRate(baudRate) { } - void begin() { serialPort.begin(baudRate, SERIAL_8N1, rxPin, txPin); } - - void setChannels(const uint16_t *channels, size_t numChannels) + void begin() { - const uint8_t DEVICE_ADDRESS = 0xC8; // Transmitter address - const uint8_t TYPE_CHANNEL_DATA = 0x16; // Channel data type - const size_t PAYLOAD_SIZE = - (numChannels * 11 + 7) / 8; // Calculate payload size for 11 bits per channel - const size_t PACKET_SIZE = - 4 + PAYLOAD_SIZE; // Address, type, length, payload, CRC + // Initialize the UART. If you want two-way, specify both rxPin, txPin. + // Example: 8N1, no inversion, 20s timeout + serialPort.begin(baudRate, SERIAL_8N1, rxPin, txPin, false, 20000UL); + } + + // ------------------------------- + // Send a 16-channel RC frame (10 bits per channel). + // channelData[i] should be in the range [0..1023]. + // For typical servo-like values (1000..2000), you must scale them down if needed. + // ------------------------------- + void sendRCFrame(const uint16_t *channelData) + { + constexpr size_t CHANNEL_BITS = 10; + constexpr size_t CHANNEL_COUNT = CRSF_MAX_CHANNELS; + constexpr size_t CHANNEL_BYTES = + ((CHANNEL_COUNT * CHANNEL_BITS) + 7) / 8; // => 20 + constexpr size_t FRAME_TYPE_SIZE = 1; // 1 byte for frame type (0x16) + constexpr size_t CRC_SIZE = 1; // 1 byte for CRC + constexpr size_t CRSF_PAYLOAD_LEN = + FRAME_TYPE_SIZE + CHANNEL_BYTES + CRC_SIZE; // => 1 + 20 + 1 = 22 + constexpr size_t PACKET_SIZE = 2 + CRSF_PAYLOAD_LEN; // => 24 total uint8_t packet[PACKET_SIZE]; - packet[0] = DEVICE_ADDRESS; - packet[1] = PAYLOAD_SIZE + 2; // Length includes type and CRC - packet[2] = TYPE_CHANNEL_DATA; - // Pack the channel data into the payload - uint8_t bitsMerged = 0; - uint32_t readValue = 0; - unsigned writeIndex = 3; - for (size_t i = 0; i < numChannels; ++i) + // Address (sync) byte + packet[0] = CRSF_SYNC_BYTE; + // Length (22) + packet[1] = CRSF_PAYLOAD_LEN; + // Frame Type + packet[2] = CRSF_FRAME_TYPE_CHANNELS; + + // Pack channel data (16 × 10 bits = 160 bits => 20 bytes) + uint32_t bitBuffer = 0; + uint8_t bitCount = 0; + size_t byteIndex = 3; // start filling after [0..2] + + for (size_t i = 0; i < CHANNEL_COUNT; i++) { - readValue |= ((uint32_t)channels[i] & 0x7FF) << bitsMerged; - bitsMerged += 11; - while (bitsMerged >= 8) + uint16_t val = channelData[i] & 0x03FF; // 10-bit mask + bitBuffer |= (static_cast(val) << bitCount); + bitCount += CHANNEL_BITS; // +10 + + while (bitCount >= 8) { - packet[writeIndex++] = readValue & 0xFF; - readValue >>= 8; - bitsMerged -= 8; + packet[byteIndex++] = static_cast(bitBuffer & 0xFF); + bitBuffer >>= 8; + bitCount -= 8; } } - if (bitsMerged > 0) + + // If leftover bits remain, put them into the last byte + if (bitCount > 0) { - packet[writeIndex++] = readValue & 0xFF; + packet[byteIndex++] = static_cast(bitBuffer & 0xFF); } - // Calculate CRC - uint8_t crc = calculateCRC(packet, PACKET_SIZE - 1); + // Compute CRC over [Type + channel bytes] => 21 bytes + const size_t CRC_LENGTH = CRSF_PAYLOAD_LEN - 1; // (22 - 1) = 21 + uint8_t crc = calculateCRC(&packet[2], CRC_LENGTH); + // Place CRC at last byte (index 23) packet[PACKET_SIZE - 1] = crc; - // Send the packet + // Send out the 24-byte CRSF RC frame serialPort.write(packet, PACKET_SIZE); } + // ------------------------------- + // Poll the serial port for incoming CRSF frames. + // If we receive a valid 16-ch "Channel Data" frame (type=0x16), + // decode the channels into the provided channelData[] array. + // Returns 'true' if a new channel frame was successfully parsed. + // + // Call this in your loop() or in a periodic task. + // ------------------------------- + bool receiveRCFrame(uint16_t *channelData) + { + // Run our state machine as long as there's data available + while (serialPort.available() > 0) + { + uint8_t c = static_cast(serialPort.read()); + + switch (rxState) + { + case WAIT_SYNC: + if (c == CRSF_SYNC_BYTE) + { + rxPacket[0] = c; + rxIndex = 1; + rxState = WAIT_LEN; + } + break; + + case WAIT_LEN: + // The next byte after sync is "length" + rxPacket[rxIndex++] = c; + rxLength = c; // length should be the payload from [type..CRC] + // Basic sanity check: max 64 or so. CRSF frames can vary, but let's be + // safe. + if (rxLength < 2 || rxLength > 64) + { + // Invalid length, reset parser + rxState = WAIT_SYNC; + rxIndex = 0; + } + else + { + // Next step: read length more bytes + rxState = WAIT_PAYLOAD; + } + break; + + case WAIT_PAYLOAD: + // Store the byte + rxPacket[rxIndex++] = c; + + // If we've reached the entire frame, parse + if (rxIndex >= (2 + rxLength)) + { + // We have [0]=0xC8, [1]=length, plus 'length' bytes + // Check CRC, parse if valid + bool ok = parseFrame(channelData); + // Reset to look for the next frame + rxState = WAIT_SYNC; + rxIndex = 0; + + if (ok) + { + // We got a valid channel frame + return true; + } + } + break; + } + } + + return false; // no new channel frame parsed + } + private: HardwareSerial &serialPort; - int txPin; int rxPin; + int txPin; long baudRate; + // ---- Receive State Machine Fields ---- + enum RxState + { + WAIT_SYNC, + WAIT_LEN, + WAIT_PAYLOAD + } rxState = WAIT_SYNC; + + uint8_t rxPacket[64]; // buffer for incoming frame + size_t rxIndex = 0; // how many bytes we've stored + uint8_t rxLength = 0; // length byte from the packet + + // 8-bit CRC with polynomial 0xD5 uint8_t calculateCRC(const uint8_t *data, size_t length) { uint8_t crc = 0; - for (size_t i = 0; i < length; ++i) + for (size_t i = 0; i < length; i++) { crc ^= data[i]; + for (uint8_t bit = 0; bit < 8; bit++) + { + if (crc & 0x80) + { + crc <<= 1; + crc ^= 0xD5; + } + else + { + crc <<= 1; + } + } } return crc; } + + // Attempt to parse the frame in rxPacket[]. + // If it's a valid 16-ch RC frame (type=0x16) and CRC checks out, + // decode into channelData[]. + // Returns true on success, false otherwise. + bool parseFrame(uint16_t *channelData) + { + // Basic layout: + // rxPacket[0] = 0xC8 + // rxPacket[1] = length (n) + // Then we have n bytes: [2..(1+n)] + // The last of those n bytes is CRC. + + uint8_t lengthField = rxPacket[1]; + // The "payload" = lengthField bytes from rxPacket[2]..rxPacket[1 + lengthField] + // So the CRC is at rxPacket[1 + lengthField]. + uint8_t crc = rxPacket[1 + lengthField]; + + // Check CRC over the [Frame Type..(payload)] excluding the CRC byte + // i.e. from rxPacket[2]..rxPacket[1 + lengthField - 1] + size_t dataLen = lengthField - 1; // e.g. if length=22, dataLen=21 + uint8_t calcCRC = calculateCRC(&rxPacket[2], dataLen); + + if (calcCRC != crc) + { + // CRC mismatch + return false; + } + + // Check frame type + uint8_t frameType = rxPacket[2]; + if (frameType != CRSF_FRAME_TYPE_CHANNELS) + { + // Not a channel frame + return false; + } + + // For a 16-ch frame, lengthField should be 22 + // but let's check if we want to be sure + if (lengthField != 22) + { + // not a 16-ch RC frame + return false; + } + + // If we reach here, we have a valid 16-ch frame + // Decode the 20 bytes of channel data + // They live at rxPacket[3..22], which is 20 bytes + decodeChannels(&rxPacket[3], channelData); + return true; + } + + // Decode 16 channels of 10 bits from the 20-byte payload + // into channelData[16]. + void decodeChannels(const uint8_t *payload, uint16_t *channelData) + { + constexpr size_t CHANNEL_COUNT = CRSF_MAX_CHANNELS; // 16 + constexpr size_t CHANNEL_BITS = 10; + + uint32_t bitBuffer = 0; + uint8_t bitCount = 0; + size_t bytePos = 0; + + for (size_t i = 0; i < CHANNEL_COUNT; i++) + { + // accumulate bits until we have at least 10 + while (bitCount < CHANNEL_BITS) + { + bitBuffer |= (static_cast(payload[bytePos++]) << bitCount); + bitCount += 8; + } + // extract 10 bits + uint16_t val = static_cast(bitBuffer & 0x3FF); // 10-bit mask + bitBuffer >>= CHANNEL_BITS; + bitCount -= CHANNEL_BITS; + + // store + channelData[i] = val; + } + } }; diff --git a/radio/FHSS.cpp b/radio/FHSS.cpp index f9a8eba..25aa43a 100644 --- a/radio/FHSS.cpp +++ b/radio/FHSS.cpp @@ -4,7 +4,13 @@ float hopTable[MAX_HOP_CHANNELS]; uint64_t packetNumber = 0; long int receivedPacketCounter = 0; -uint32_t syncWord = 0x1A2B3C4D; // Example sync word (can be any 32-bit value) +uint32_t syncWord = 98754386857476; // Example sync word +// uint32_t syncWord = 0x1A2B3C4D; // Example sync word (can be any 32-bit value) + +float maxWidthMHz = 5.0; // Max hopping width of 20 MHz +float startFreq = LORA_BASE_FREQ - maxWidthMHz; // Start at 900 MHz +float stepKHz = 10.0; // 10 kHz step size + int numChannels = 0; // Get the next frequency from the hopping table diff --git a/radio/FHSS.h b/radio/FHSS.h index 953926b..c188f6d 100644 --- a/radio/FHSS.h +++ b/radio/FHSS.h @@ -1,3 +1,4 @@ +#include "config.h" #include #include #include @@ -20,6 +21,11 @@ extern unsigned long lastHopTime; extern unsigned long dwellTime; // 500ms dwell time extern float currentFreq; +extern uint32_t syncWord; // Example sync word +extern float maxWidthMHz; // Max hopping width of 20 MHz +extern float startFreq; // Start at 900 MHz +extern float stepKHz; // 10 kHz step size + // Function to generate a frequency hopping table, adapting if channels are fewer int generateFrequencies(uint32_t syncWord, float startFreq, float stepKHz, float maxWidthMHz); diff --git a/radio/MavLink.h b/radio/MavLink.h new file mode 100644 index 0000000..f66a81f --- /dev/null +++ b/radio/MavLink.h @@ -0,0 +1,125 @@ +#include + +// Include MAVLink headers. +// Adjust the include path to match your local mavlink library structure. +#include "mavlink.h" + +// Which UART on ESP32? For example, Serial2 can be mapped to pins: +constexpr int RX_PIN = 16; // Flight Controller TX -> ESP32 RX +constexpr int TX_PIN = 17; // Flight Controller RX <- ESP32 TX +constexpr long MAVLINK_BAUD = 57600; // Common default for MAVLink + +// Identify our local system/component, and the target FC system +// (ArduPilot often uses sysid=1, compid=1, but it can vary). +constexpr uint8_t MAV_COMP_ID_ONBOARD = 1; // or 190 for companion computer +constexpr uint8_t MAV_SYS_ID_ONBOARD = 255; // e.g. 255 for companion +constexpr uint8_t TARGET_SYS_ID = 1; // autopilot system ID +constexpr uint8_t TARGET_COMP_ID = 1; // autopilot component ID + +// We'll send 8 channels. MAVLink 2 can handle up to 18 channels in +// set_rc_channels_override, but typical autopilots read at least 8 or 14 channels. +static uint16_t rcChannels[8] = { + 1500, // Channel 1 + 1500, // Channel 2 + 1500, // Channel 3 + 1500, // Channel 4 + 1000, // Channel 5 + 1000, // Channel 6 + 1000, // Channel 7 + 1000 // Channel 8 +}; + +void setup() +{ + Serial.begin(115200); + delay(2000); + Serial.println("MAVLink RC Override Example - ESP32"); + + // Initialize Serial2 on given pins + Serial2.begin(MAVLINK_BAUD, SERIAL_8N1, RX_PIN, TX_PIN); + + // (Optional) Wait a bit for the autopilot to boot, if needed + delay(2000); +} + +void loop() +{ + // For demonstration, let's do a small "animation" on channel 1 + // to show that the flight controller is indeed receiving changes. + // We'll move channel 1 from 1000 to 2000 in steps. + static int ch1Value = 1000; + static int increment = 10; + + ch1Value += increment; + if (ch1Value > 2000) + { + ch1Value = 2000; + increment = -10; + } + else if (ch1Value < 1000) + { + ch1Value = 1000; + increment = 10; + } + + // Update channel 1 in our array + rcChannels[0] = ch1Value; + + // Send the MAVLink Set RC Channels Override message + sendRCChannelsOverride(); + + // Send ~20 times per second + delay(50); +} + +void sendRCChannelsOverride() +{ + mavlink_message_t msg; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + // Fill out the message: + // mavlink_msg_set_rc_channels_override_pack( + // uint8_t system_id, + // uint8_t component_id, + // mavlink_message_t* msg, + // uint8_t target_system, + // uint8_t target_component, + // uint16_t chan1_raw, + // uint16_t chan2_raw, + // ... + // uint16_t chan8_raw, + // uint16_t chan9_raw, + // ... + // up to chan18_raw + // ) + // For 8 channels, pass 0 for unused channels > 8. + + mavlink_msg_set_rc_channels_override_pack( + MAV_SYS_ID_ONBOARD, MAV_COMP_ID_ONBOARD, &msg, TARGET_SYS_ID, TARGET_COMP_ID, + rcChannels[0], // chan1 + rcChannels[1], // chan2 + rcChannels[2], // chan3 + rcChannels[3], // chan4 + rcChannels[4], // chan5 + rcChannels[5], // chan6 + rcChannels[6], // chan7 + rcChannels[7], // chan8 + 0, 0, 0, 0, 0, 0, 0, 0 // for chan9..chan16 or up to chan18 + ); + + // Encode the message into the send buffer + uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + + // Write it out the serial port to the FC + Serial2.write(buf, len); + + // For debugging + Serial.print("Sent RC override: ["); + for (int i = 0; i < 8; i++) + { + Serial.print(rcChannels[i]); + if (i < 7) + Serial.print(", "); + } + Serial.println("]"); +} diff --git a/radio/RadioCommands.h b/radio/RadioCommands.h index 2d90a9b..6ac846a 100644 --- a/radio/RadioCommands.h +++ b/radio/RadioCommands.h @@ -1,4 +1,6 @@ +#pragma once + #include #include #include diff --git a/radio/Readme.txt b/radio/Readme.txt index 7031e34..a709bae 100644 --- a/radio/Readme.txt +++ b/radio/Readme.txt @@ -18,3 +18,11 @@ Download the appropriate version for your operating system (Windows, macOS, or L go to Port -> select port -> check Serial RX Go to + +# SBUS tested + +ESP32 39 -> SBUS(R2) Any of them +GND -> G +Article explains : https://speedybee.zendesk.com/hc/en-us/articles/19968381088795-How-to-set-up-your-SBUS-receiver-in-Betaflight-configurator-on-SpeedyBee-F405MINI-flight-controller + + diff --git a/radio/SBUS.h b/radio/SBUS.h new file mode 100644 index 0000000..4b6515c --- /dev/null +++ b/radio/SBUS.h @@ -0,0 +1,63 @@ +#include "config.h" +#include + +#define SBUS_CHANNELS 16 +#define SBUS_PACKET_SIZE 25 + +class SBUS2 +{ + public: + SBUS2(HardwareSerial &serialPort) : sbusSerial(serialPort) + { + sbusSerial.begin(100000, SERIAL_8E2, -1, TXD1, true); // Inverted SBUS signal + } + + void send(uint16_t channels[SBUS_CHANNELS]) + { + uint8_t sbusPacket[SBUS_PACKET_SIZE] = {0}; + + sbusPacket[0] = 0x0F; // SBUS Start Byte + + // Correctly pack 16 channels (11-bit each) into 22 bytes + uint16_t packedData[SBUS_CHANNELS] = {0}; + for (int i = 0; i < SBUS_CHANNELS; i++) + { + packedData[i] = channels[i] & 0x07FF; // Ensure 11-bit limit + } + + sbusPacket[1] = (packedData[0] & 0xFF); + sbusPacket[2] = ((packedData[0] >> 8) | (packedData[1] << 3)) & 0xFF; + sbusPacket[3] = ((packedData[1] >> 5) | (packedData[2] << 6)) & 0xFF; + sbusPacket[4] = (packedData[2] >> 2) & 0xFF; + sbusPacket[5] = ((packedData[2] >> 10) | (packedData[3] << 1)) & 0xFF; + sbusPacket[6] = ((packedData[3] >> 7) | (packedData[4] << 4)) & 0xFF; + sbusPacket[7] = ((packedData[4] >> 4) | (packedData[5] << 7)) & 0xFF; + sbusPacket[8] = (packedData[5] >> 1) & 0xFF; + sbusPacket[9] = ((packedData[5] >> 9) | (packedData[6] << 2)) & 0xFF; + sbusPacket[10] = ((packedData[6] >> 6) | (packedData[7] << 5)) & 0xFF; + sbusPacket[11] = (packedData[7] >> 3) & 0xFF; + sbusPacket[12] = (packedData[8] & 0xFF); + sbusPacket[13] = ((packedData[8] >> 8) | (packedData[9] << 3)) & 0xFF; + sbusPacket[14] = ((packedData[9] >> 5) | (packedData[10] << 6)) & 0xFF; + sbusPacket[15] = (packedData[10] >> 2) & 0xFF; + sbusPacket[16] = ((packedData[10] >> 10) | (packedData[11] << 1)) & 0xFF; + sbusPacket[17] = ((packedData[11] >> 7) | (packedData[12] << 4)) & 0xFF; + sbusPacket[18] = ((packedData[12] >> 4) | (packedData[13] << 7)) & 0xFF; + sbusPacket[19] = (packedData[13] >> 1) & 0xFF; + sbusPacket[20] = ((packedData[13] >> 9) | (packedData[14] << 2)) & 0xFF; + sbusPacket[21] = ((packedData[14] >> 6) | (packedData[15] << 5)) & 0xFF; + sbusPacket[22] = (packedData[15] >> 3) & 0xFF; + + // Flags: Failsafe & Frame Lost + sbusPacket[23] = 0x00; // Set failsafe/lost frame bits here if needed + + // End byte + sbusPacket[24] = 0x00; // Must be 0x00 or 0x04 if failsafe active + + // Send SBUS packet + sbusSerial.write(sbusPacket, SBUS_PACKET_SIZE); + } + + private: + HardwareSerial &sbusSerial; +}; diff --git a/radio/Tx.cpp b/radio/Tx.cpp index e6dcbd5..525778c 100644 --- a/radio/Tx.cpp +++ b/radio/Tx.cpp @@ -1,77 +1,20 @@ +#include "USB-Serial.h" +#include "config.h" +#include "radio-protocol.h" +#include "utility.h" #include #include #include #include #include -#include #include #include #include -#define RUN_TESTS 0 - -#ifndef LORA_SF -// Sets LoRa spreading factor. Allowed values range from 5 to 12. -#define LORA_SF 8 -#endif - -#ifndef LORA_CR -#define LORA_CR 8 -#endif - -#ifndef DEBUG_RX -#define DEBUG_RX 0 -#endif - -#ifndef LORA_HEADER -#define LORA_HEADER 0 -#endif - -#ifndef LORA_FHSS -#define LORA_FHSS 0 -#endif - -#ifndef LORA_RX -#define LORA_RX 0 -#endif - -#ifndef LORA_TX -#define LORA_TX 1 -#endif - -#ifndef LORA_BASE_FREQ -// Sets LoRa coding rate denominator. Allowed values range from 5 to 8. -#define LORA_BASE_FREQ 915 -#endif - -#ifndef LORA_BW -// Sets LoRa bandwidth. Allowed values are 62.5, 125.0, 250.0 and 500.0 kHz. (default, -// high = false) -#define LORA_BW 62.5 // 31.25 // 125.0 // 62.5 // 31.25 -#endif - -#ifndef LORA_DATA_BYTE -#define LORA_DATA_BYTE 2 -#endif - -#define SBUS 1 // Doesn't work -#define IBUS 2 -#define CROS 3 - -#ifndef PROTOCOL -#define PROTOCOL CROS // IBUS // SBUS -#endif - -#ifndef LORA_PREAMBLE -// 8 is default -#if LORA_SF == 6 || LORA_SF == 5 -#define LORA_PREAMBLE 8 -#elif LORA_SF == 7 -#define LORA_PREAMBLE 8 -#else -#define LORA_PREAMBLE 10 -#endif -#endif +// Support heltec boards +#ifndef LILYGO +#include +#endif // end ifndef LILYGO #if defined(LILYGO) // LiLyGO device does not support the auto download mode, you need to get into the @@ -95,13 +38,10 @@ #define TXD1 39 // Transmit pin for Serial1 #define RXD2 40 // Receive pin for Serial2 -String readSerialInput(); -std::map processSerialCommand(const String &input); - long int lastWriteTime = 0; #if PROTOCOL == IBUS -#include "i-bus.h" +#include "IBUS.h" // Create an instance of the Ibus class Ibus ibus; // Test data list with all control values set to 1700 @@ -109,144 +49,26 @@ uint8_t testControlValues[IBUS_CHANNELS_COUNT * 2]; #endif #if PROTOCOL == SBUS -#include "s-bus.h" +#include "SBUS.h" +SBUS2 sbus(Serial1); // Use Serial1 for SBUS output #endif #include "FHSS.h" -#if PROTOCOL == CROS +#if PROTOCOL == CRSF #include "CRSF.h" -CRSF crsf(Serial1, TXD1, -1, 420000); // Use Serial1, TX_PIN, RX_PIN, BAUD_RATE +// CRSF crsf(Serial1, TXD1, TXD1, 420000); // Use Serial1, TX_PIN, RX_PIN, BAUD_RATE +CRSF2 crsf(Serial1, -1, TXD1); #endif // Example usage -int packetSave = 0; -bool packetReceived = false; -// Function to send 2-byte LoRa packet OR 4-byte LoRa packet -void sendLoRaRCPacket(uint8_t cmd1, uint8_t val1, uint8_t cmd2, uint8_t val2, - uint8_t cmd3 = 0, uint8_t val3 = 0, uint8_t cmd4 = 0, - uint8_t val4 = 0) -{ - cmd1 &= 0x0F; - val1 &= 0x0F; - cmd2 &= 0x0F; - val2 &= 0x0F; - int packetSize = LORA_DATA_BYTE; - if (cmd3 != 0 && val3 != 0) - { - cmd3 &= 0x0F; - val3 &= 0x0F; - cmd4 &= 0x0F; - val4 &= 0x0F; - packetSize = 4; - } +#include "radio.h" - Serial.printf("Sending LoRa Packet: CMD1=%d, VAL1=%d, CMD2=%d, VAL2=%d, ", cmd1, val1, - cmd2, val2); - if (packetSize == 4) - { - Serial.printf("CMD3=%d, VAL3=%d, CMD4=%d, VAL4=%d, ", cmd3, val3, cmd4, val4); - } - Serial.println(); - - uint8_t paddedData[packetSize] = {0}; // Initialize with zeros - // P:2:5:4:10 - // 0010:0101:0100:1010 - // RX 11001110 - // 9546 - // 0010:0101:0100:1010 - uint16_t packet = (cmd1 << 12) | (val1 << 8) | (cmd2 << 4) | val2; - uint16_t packet2 = 0; - - if (packetSize == 4) - { - packet2 = (cmd3 << 12) | (val3 << 8) | (cmd4 << 4) | val4; - } - - packetSave = packet; - if (packet2 != 0) - { - packetSave = (static_cast(packet) << 16) | packet2; - } - - uint8_t data[packetSize] = {0}; - data[0] = (packet >> 8) & 0xFF; // High byte - data[1] = packet & 0xFF; // Low byte - - if (packetSize == 4) - { - data[2] = (packet2 >> 8) & 0xFF; // High byte - data[3] = packet2 & 0xFF; // Low byte - } - - int len = sizeof(data); - - Serial.println("Packet length: " + String(len)); - Serial.print("Sending Packet: "); - Serial.print(data[0], BIN); - Serial.print(" "); - Serial.print(data[1], BIN); - if (packetSize == 4) - { - Serial.print(" "); - Serial.print(data[2], BIN); - Serial.print(" "); - Serial.print(data[3], BIN); - } - Serial.println(); - - // size_t dataSize = sizeof(data) / sizeof(data[0]); - // memcpy(paddedData, data, min(dataSize, (size_t)LORA_DATA_BYTE)); - - int status = radio.transmit(data, sizeof(data)); - if (status == RADIOLIB_ERR_NONE) - { - Serial.println("LoRa Packet Sent!"); - } - else - { - Serial.println("LoRa Transmission Failed."); - } -} - -void sendLoRaPacket(uint8_t *packetData, int length) -{ - // Size of pointer here not an array count. - // int length = sizeof(packetData); - String packetStr = ""; - - Serial.println("Packet length: " + String(length)); - - Serial.print("Sending Packet: "); - for (size_t i = 0; i < length; i++) - { - Serial.print(packetData[i], BIN); - Serial.print(" "); - packetStr += String(packetData[i]); - } - Serial.println(); - display.println("P[" + String(length) + "]:" + packetStr); - Serial.println("P[" + String(length) + "]:" + packetStr); - - int status = radio.transmit(packetData, length); - if (status == RADIOLIB_ERR_NONE) - { - Serial.println("LoRa Packet Sent!"); - } - else - { - Serial.println("LoRa Transmission Failed."); - } -} - -void onReceive(); -void onReceiveFlag(void); #if RUN_TESTS void testMap11BitTo4Bit(); void testMap4BitTo11Bit(); #endif -bool radioIsRX = false; long int startTime = 0; void setup() { @@ -260,13 +82,13 @@ void setup() #endif #if PROTOCOL == SBUS - clearSbusData(); + // clearSbusData(); #if LORA_RX - sbusWrite.Begin(); + // sbusWrite.Begin(); #endif #endif -#if PROTOCOL == CROS +#if PROTOCOL == CRSF crsf.begin(); #endif // end CRSF @@ -278,18 +100,13 @@ void setup() // Initialize SBUS communication #if LORA_TX - sbusRead.Begin(); + // sbusRead.Begin(); #endif Serial.println("SBUS write and read are ready"); heltec_setup(); startTime = millis(); - uint32_t syncWord = 98754386857476; // Example sync word - float maxWidthMHz = 5.0; // Max hopping width of 20 MHz - float startFreq = LORA_BASE_FREQ - maxWidthMHz; // Start at 900 MHz - float stepKHz = 10.0; // 10 kHz step size - numChannels = generateFrequencies(syncWord, startFreq, stepKHz, maxWidthMHz); delay(100); @@ -321,94 +138,33 @@ void setup() ; } - radio.setFrequency(LORA_BASE_FREQ); - radio.setBandwidth(LORA_BW); - radio.setSpreadingFactor(LORA_SF); -#if LORA_HEADER - // Some issue it receives 0 0 - radio.implicitHeader(LORA_DATA_BYTE); -#else - radio.explicitHeader(); -#endif - radio.setCodingRate(LORA_CR); - radio.setPreambleLength(LORA_PREAMBLE); - radio.forceLDRO(true); - radio.setCRC(2); -#if LORA_TX - radio.setOutputPower(22); - display.println("Turn ON RX to pair you have 30 seconds"); - for (int t = 0; t < 30; t++) + // Setup Radio + setupRadio(); +} + +String toBinary(int num, int bitSize = 4); +std::map lastSerialCommands; +void loop() +{ +// Read serial input +#if LORA_TX && SERIAL_INPUT + Serial.println("Waiting for Serial Commands"); + while (true) { - display.print("."); - delay(50); - } - display.println(); -#endif - -#ifdef USING_LR1121 // USING_SX1262 - // LR1121 - // set RF switch configuration for Wio WM1110 - // Wio WM1110 uses DIO5 and DIO6 for RF switching - static const uint32_t rfswitch_dio_pins[] = {RADIOLIB_LR11X0_DIO5, - RADIOLIB_LR11X0_DIO6, RADIOLIB_NC, - RADIOLIB_NC, RADIOLIB_NC}; - - static const Module::RfSwitchMode_t rfswitch_table[] = { - // mode DIO5 DIO6 - {LR11x0::MODE_STBY, {LOW, LOW}}, {LR11x0::MODE_RX, {HIGH, LOW}}, - {LR11x0::MODE_TX, {LOW, HIGH}}, {LR11x0::MODE_TX_HP, {LOW, HIGH}}, - {LR11x0::MODE_TX_HF, {LOW, LOW}}, {LR11x0::MODE_GNSS, {LOW, LOW}}, - {LR11x0::MODE_WIFI, {LOW, LOW}}, END_OF_MODE_TABLE, - }; - radio.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table); - - // LR1121 TCXO Voltage 2.85~3.15V - radio.setTCXO(3.0); - heltec_delay(500); -#endif - -#if LORA_RX - radio.setPacketReceivedAction(onReceiveFlag); - int state = radio.startReceive(); - if (state == RADIOLIB_ERR_NONE) - { - radioIsRX = true; - Serial.println("Listening for LoRa Packets..."); - } - else - { - Serial.print("Receiver failed to start, code: "); - Serial.println(state); - while (true) + std::map serialCommands = readSerialInput(); + if (serialCommands.empty()) { - delay(5); + // Serial.println("Waiting for Serial Commands"); + delay(20); + } + else + { + lastSerialCommands = serialCommands; + break; } } #endif -} -void forceRestartLoRa(); -String toBinary(int num, int bitSize = 4); - -unsigned long lastPacketTime = 0; -long int packetN = 0; -void loop() -{ - String input = readSerialInput(); - std::map result = {}; - if (!input.isEmpty()) - { - result = processSerialCommand(input); - } - - if (result.empty()) - { - Serial.println("The map is empty."); - } - else - { - Serial.println("The map contains data."); - } #if PROTOCOL == IBUS uint32_t seed = esp_random() ^ millis(); randomSeed(seed); @@ -423,9 +179,11 @@ void loop() testControlValues[i * 2 + 1] = (randomValue >> 8) & 0xFF; // High byte } Serial.println("I-BUS:" + str); + display.println("I-BUS:" + str); ibus.setControlValuesList(testControlValues); ibus.sendPacket(); + delay(20); #endif // end IBUS #if PROTOCOL == SBUS @@ -441,17 +199,50 @@ void loop() uint16_t randomValue = random(1200, 1900); sbusSend[i] = randomValue; // map4BitTo11Bit(randomValue); } - writeSbusData(sbusSend); - // delay(500); + // writeSbusData(sbusSend); + sbus.send(sbusSend); + display.println("SBUS"); + Serial.println("SBUS: " + String(sbusSend[0]) + "," + String(sbusSend[1]) + "," + + String(sbusSend[2]) + "," + String(sbusSend[3]) + "," + + String(sbusSend[4]) + "," + String(sbusSend[5]) + "," + + String(sbusSend[6]) + "," + String(sbusSend[7]) + "," + + String(sbusSend[8]) + "," + String(sbusSend[9]) + "," + + String(sbusSend[10]) + "," + String(sbusSend[11])); + delay(30); // Read data for test purpose // readSbusData(); #endif // end SBUS -#if PROTOCOL == CROS +#if PROTOCOL == CRSF + display.println("CRSF"); + uint32_t seed = esp_random() ^ millis(); + + randomSeed(seed); + // Set all control values to 1700 + + uint16_t sbusSend[16] = INIT_SBUS_ARRAY; // Example: Set channel values - uint16_t channels[] = {1700, 1800, 1600, 1200, - 1580, 1600, 1300, 1900}; // Example channel values - crsf.setChannels(channels, sizeof(channels) / sizeof(channels[0])); + uint16_t channels[CRSF_MAX_CHANNELS] = {random(1200, 1900), + random(1200, 1900), + random(1200, 1900), + random(1200, 1900), + random(1200, 1900), + random(1200, 1900), + random(1200, 1900), + 1435, + 1450, + 1800, + 1300, + 1000, + 1500, + 2000, + 1200, + 1300}; + + crsf.sendRCFrame(channels); + Serial.println("CRSF: " + String(channels[0]) + "," + String(channels[1]) + "," + + String(channels[2]) + "," + String(channels[3])); + delay(50); #endif // end CRSF uint8_t cmd1 = 0; // Example command 1 @@ -466,19 +257,28 @@ void loop() #endif #if LORA_TX // listen to the Sbus commands form the Ground station or RC - readSbusData(); + // readSbusData(); - val1 = convertTo4Bit(sbusDataRead.ch[0]); - val2 = convertTo4Bit(sbusDataRead.ch[1]); + val1 = convertTo4Bit(testChannels[4]); + val2 = convertTo4Bit(testChannels[1]); // 4 byte packet - val3 = convertTo4Bit(sbusDataRead.ch[2]); - val4 = convertTo4Bit(sbusDataRead.ch[3]); + val3 = convertTo4Bit(testChannels[2]); + val4 = convertTo4Bit(testChannels[3]); if (val3 != 8 && val4 != 8) { + radio.setBandwidth(62.5); + // RC 1 1500 + sendLoRaRCPacket(cmd1, val1, cmd2, val2, cmd3, val3, cmd4, val4); + delay(500); + radio.setBandwidth(500); sendLoRaRCPacket(cmd1, val1, cmd2, val2, cmd3, val3, cmd4, val4); } else { + radio.setBandwidth(62.5); + sendLoRaRCPacket(cmd1, val1, cmd2, val2); + delay(500); + radio.setBandwidth(500); sendLoRaRCPacket(cmd1, val1, cmd2, val2); } @@ -579,251 +379,3 @@ void loop() #if defined(ESP8266) || defined(ESP32) ICACHE_RAM_ATTR #endif - -void onReceiveFlag(void) { packetReceived = true; } - -void onReceive(void) -{ -#if LORA_RX - receivedPacketCounter++; - size_t len = radio.getPacketLength(true); - uint8_t data[len] = {0}; - Serial.println("[LoRa] onReceive(" + String(len) + ")"); - -#if DEBUG_RX - Serial.println("[LoRa] onReceive"); - if (len > LORA_DATA_BYTE) - { - Serial.println("WARNING: Packet size is too large:" + String(len)); - } - - Serial.println("[LoRa] Length: " + String(len)); -#endif - int state = radio.readData(data, len); - - if (state == RADIOLIB_ERR_NONE) - { -#if DEBUG_RX - if (sizeof(data) != LORA_DATA_BYTE) - { - Serial.println("[LoRa] ERROR: Packet Length Mismatch!"); - } -#endif - // Packet size 4 processing - if (len == 8) - { - uint64_t seqNum = (static_cast(data[0]) << 56) | - (static_cast(data[1]) << 48) | - (static_cast(data[2]) << 40) | - (static_cast(data[3]) << 32) | - (static_cast(data[4]) << 24) | - (static_cast(data[5]) << 16) | - (static_cast(data[6]) << 8) | - static_cast(data[7]); -#if DEBUG - - Serial.println("Lost:" + String(seqNum) + "/" + - String(receivedPacketCounter) + ":" + - String(seqNum - receivedPacketCounter)); -#endif - int lost = seqNum - receivedPacketCounter; - display.println("Lost:" + String(seqNum) + "/" + - String(receivedPacketCounter) + ":" + String(lost)); - } - - // Check if data is actually zero - else if (data[0] == 0 && data[1] == 0) - { - int length = len; -#if DEBUG - Serial.println("[LoRa] WARNING: Received 0 : 0. I don't know why"); - Serial.print("[LoRa Receiver] Packet length: "); - - Serial.println(String(length)); - - // return; // Ignore this packet - Serial.print("[LoRa Receiver] RSSI: "); - - Serial.print(radio.getRSSI()); - // display.println("0-0"); - - Serial.print("[LoRa Receiver] SNR: "); - Serial.print(radio.getSNR()); - Serial.println(" dB"); -#endif - } - else if (len == 2 || len == 4) //** ToDo: process length 4 */) - { - int length = len; -#if DEBUG - Serial.println("[LoRa Receiver] Packet Received!"); - Serial.print("[LoRa Receiver] Packet length: "); - Serial.println(String(length)); - Serial.println("[LoRa Receiver] DATA: " + String(data[0]) + ":" + - String(data[1])); -#endif - display.println("DATA[" + String(packetN) + "]: " + String(data[0]) + ":" + - String(data[1])); - if (len == 4) - { - display.println("DATA-4[" + String(packetN) + "]: " + String(data[2]) + - ":" + String(data[3])); - } - packetN++; - // Decode received data - uint16_t receivedPacket = (data[0] << 8) | data[1]; - uint8_t cmd1 = (receivedPacket >> 12) & 0x0F; - uint8_t val1 = (receivedPacket >> 8) & 0x0F; - uint8_t cmd2 = (receivedPacket >> 4) & 0x0F; - uint8_t val2 = receivedPacket & 0x0F; - - display.println("Data:" + String(cmd1) + ":" + String(val1) + ":" + - String(cmd2) + ":" + (val2)); - - // If size is 4, decode another 16 bits - uint8_t cmd3 = 0, val3 = 0, cmd4 = 0, val4 = 0; - - if (len == 4) - { - uint16_t receivedPacket2 = (data[2] << 8) | data[3]; - uint8_t cmd3 = (receivedPacket2 >> 12) & 0x0F; - uint8_t val3 = (receivedPacket2 >> 8) & 0x0F; - uint8_t cmd4 = (receivedPacket2 >> 4) & 0x0F; - uint8_t val4 = receivedPacket2 & 0x0F; - - display.println("Data:" + String(cmd3) + ":" + String(val3) + ":" + - String(cmd4) + ":" + (val4)); - } - -#if DEBUG - // Print received data - Serial.print("[LoRa Receiver] Data: "); - Serial.print("CMD1="); - Serial.print(cmd1); - Serial.print(", VAL1="); - Serial.print(val1); - Serial.print(", CMD2="); - Serial.print(cmd2); - Serial.print(", VAL2="); - Serial.println(val2); - Serial.print("[LoRa Receiver] Data: "); - - // Print RSSI (Signal Strength) - Serial.print("[LoRa Receiver] RSSI: "); - Serial.print(radio.getRSSI()); - Serial.println(" dBm"); - - // Print SNR (Signal-to-Noise Ratio) - Serial.print("[LoRa Receiver] SNR: "); - Serial.print(radio.getSNR()); - Serial.println(" dB"); -#endif - display.print("RSSI: " + String(radio.getRSSI())); - display.println(" SNR: " + String(radio.getSNR())); - } - } - else if (state == RADIOLIB_ERR_RX_TIMEOUT) - { - // No packet received - Serial.println("[LoRa Receiver] No packet received."); - } - else if (state == RADIOLIB_ERR_CRC_MISMATCH) - { - // Packet received but corrupted - Serial.println("[LoRa Receiver] Packet received but CRC mismatch!"); - } - else - { - // Other error - Serial.print("[LoRa Receiver] Receive failed, error code: "); - Serial.println(state); - } - // Restart LoRa receiver - // radio.implicitHeader(LORA_DATA_BYTE); - radio.startReceive(); -#endif -} - -String toBinary(int num, int bitSize) -{ - if (num == 0) - return "0"; - - String binary = ""; - while (num > 0) - { - binary = String(num % 2) + binary; - num /= 2; - } - - // Pad with leading zeros to match `bitSize` - while (binary.length() < bitSize) - { - binary = "0" + binary; - } - - return binary; -} - -void forceRestartLoRa() -{ - Serial.println("[LoRa] Forcing Restart..."); - radio.standby(); - radio.reset(); - delay(100); - radio.begin(); - radio.startReceive(); - lastPacketTime = millis(); // Reset timeout -} - -String readSerialInput() -{ - if (Serial.available() > 0) - { - String input = Serial.readStringUntil( - '\n'); // Read the incoming data until a newline character - input.trim(); // Remove any leading or trailing whitespace - return input; - } - return ""; // Return an empty string if no data is available -} - -std::map processSerialCommand(const String &input) -{ - std::map keyValuePairs; - if (input.startsWith("RC ")) - { - int key1, value1, key2, value2; - char command[3]; - int parsed = sscanf(input.c_str(), "%s %d %d %d %d", command, &key1, &value1, - &key2, &value2); - - if (parsed == 5 && key1 >= 1 && key1 <= 16 && value1 >= 0 && value1 < 2048 && - key2 >= 1 && key2 <= 16 && value2 >= 0 && value2 < 2048) - { - // Process the command - Serial.print("Received command: "); - Serial.print(command); - Serial.print(", Key1: "); - Serial.print(key1); - Serial.print(", Value1: "); - Serial.print(value1); - Serial.print(", Key2: "); - Serial.print(key2); - Serial.print(", Value2: "); - Serial.println(value2); - - keyValuePairs[key1] = value1; - keyValuePairs[key2] = value2; - } - else - { - Serial.println("Invalid command format or out of range values."); - } - } - else - { - Serial.println("Invalid command prefix."); - } - return keyValuePairs; -} diff --git a/radio/USB-Serial.h b/radio/USB-Serial.h new file mode 100644 index 0000000..b449f45 --- /dev/null +++ b/radio/USB-Serial.h @@ -0,0 +1,96 @@ +#include +#include + +std::map readSerialInput(); +void dumpCommand(const std::map &map); +std::map processSerialCommand(const String &input); + +std::map readSerialInput() +{ + std::map result; + String input = ""; + if (Serial.available() > 0) + { + input = Serial.readStringUntil( + '\n'); // Read the incoming data until a newline character + input.trim(); // Remove any leading or trailing whitespace + } + + if (!input.isEmpty()) + { + Serial.println("SERIAL Data: " + input); + delay(1000); + result = processSerialCommand(input); + } + + if (!result.empty()) + { + Serial.println("The map contains data: "); + dumpCommand(result); + delay(1000); + } + + return result; // Return an empty string if no data is available +} + +void dumpCommand(const std::map &map) +{ + for (const auto &pair : map) + { + Serial.println("Key: " + String(pair.first) + ", Value: " + String(pair.second)); + } +} + +std::map processSerialCommand(const String &input) +{ + const int MAX_KEYS = 6; + + std::map keyValuePairs; + if (input.startsWith("RC ")) + { + int key1 = -1, value1 = -1; + int key2 = -1, value2 = -1; + int key3 = -1, value3 = -1; + int key4 = -1, value4 = -1; + int key5 = -1, value5 = -1; + int key6 = -1, value6 = -1; + + char command[3]; + int parsed = sscanf(input.c_str(), "%s %d %d %d %d %d %d %d %d %d %d %d %d", + command, &key1, &value1, &key2, &value2, &key3, &value3, + &key4, &value4, &key5, &value5, &key6, &value6); + + // Create arrays to hold keys and values + int keys[MAX_KEYS] = {key1, key2, key3, key4, key5, key6}; + int values[MAX_KEYS] = {value1, value2, value3, value4, value5, value6}; + + if (parsed >= 3) + { + Serial.print("Received command: "); + Serial.print(command); + Serial.print(" "); + for (int i = 0; i < parsed - 2; i++) + { + if (values[i] != -1) + { + keyValuePairs[keys[i]] = values[i]; + } + Serial.print("Key[" + String(i) + "]: "); + Serial.print(keys[i]); + Serial.print(" Value[" + String(i) + "]: "); + Serial.print(String(values[i]) + ", "); + } + Serial.println(" "); + Serial.println(">--------------------------------<"); + } + else + { + Serial.println("Invalid command format or out of range values."); + } + } + else + { + Serial.println("Invalid command prefix."); + } + return keyValuePairs; +} diff --git a/radio/config.h b/radio/config.h new file mode 100644 index 0000000..ff984b0 --- /dev/null +++ b/radio/config.h @@ -0,0 +1,75 @@ +#pragma once + +#define RUN_TESTS 0 + +#define TXD1 39 +#define RXD2 40 +// SBUS packet structure +#define SBUS_PACKET_SIZE 25 + +#ifndef LORA_SF +// Sets LoRa spreading factor. Allowed values range from 5 to 12. +#define LORA_SF 8 // For retranmission 6. +#endif + +#ifndef LORA_CR +#define LORA_CR 8 +#endif + +#ifndef DEBUG_RX +#define DEBUG_RX 0 +#endif + +#ifndef LORA_HEADER +#define LORA_HEADER 0 +#endif + +#ifndef LORA_FHSS +#define LORA_FHSS 0 +#endif + +#ifndef LORA_RX +#define LORA_RX 0 +#endif + +#ifndef LORA_TX +#define LORA_TX 1 +#endif + +#ifndef LORA_BASE_FREQ +// Sets LoRa coding rate denominator. Allowed values range from 5 to 8. +#define LORA_BASE_FREQ 915 +#endif + +#ifndef LORA_BW +// Sets LoRa bandwidth. Allowed values are 62.5, 125.0, 250.0 and 500.0 kHz. (default, +// high = false) +#define LORA_BW 62.5 // 31.25 // 125.0 // 62.5 // 31.25 // 500 +#endif + +#ifndef LORA_DATA_BYTE +#define LORA_DATA_BYTE 2 +#endif + +#define SBUS 1 +#define IBUS 2 +#define CRSF 3 // Doesn't work + +#ifndef PROTOCOL +#define PROTOCOL CRSF // CRSF // IBUS // SBUS +#endif + +#ifndef SERIAL_INPUT +#define SERIAL_INPUT 0 +#endif + +#ifndef LORA_PREAMBLE +// 8 is default +#if LORA_SF == 6 || LORA_SF == 5 +#define LORA_PREAMBLE 8 +#elif LORA_SF == 7 +#define LORA_PREAMBLE 8 +#else +#define LORA_PREAMBLE 10 +#endif +#endif diff --git a/radio/i-bus.h b/radio/i-bus.h deleted file mode 100644 index c70e91a..0000000 --- a/radio/i-bus.h +++ /dev/null @@ -1,161 +0,0 @@ -#include -#include -// Define constants for iBUS -#define IBUS_BAUD_RATE 115200 -#define IBUS_SEND_INTERVAL_MS 20 -#define IBUS_CHANNELS_COUNT 14 -#define IBUS_PACKET_BYTES_COUNT ((IBUS_CHANNELS_COUNT * 2) + 4) - -// Define the custom TX pin -#define CUSTOM_TX_PIN 17 // Change this to your desired TX pin - -// Define the Ibus class -class Ibus -{ - public: - void begin(HardwareSerial &serial, int txPin); - void loop(); - void enable(); - void disable(); - void readLoop(); - void sendPacket(); - bool unpackIbusData(uint8_t *packet); - void setControlValue(uint8_t channel, uint8_t value); - void setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2]); - - private: - HardwareSerial *serial; - uint8_t controlValuesList[IBUS_CHANNELS_COUNT * 2] = {0}; - bool isEnabled = false; - unsigned long previousMillis = 0; - unsigned long currentMillis = 0; - - uint8_t *createPacket(); -}; - -// Implement the Ibus methods -void Ibus::begin(HardwareSerial &serial, int txPin) -{ - this->serial = &serial; - this->serial->begin(IBUS_BAUD_RATE, SERIAL_8N1, -1, txPin); // Set custom TX pin -} - -void Ibus::loop() -{ - if (this->isEnabled) - { - this->sendPacket(); - // this->readLoop(); - } -} - -uint8_t *Ibus::createPacket() -{ - static uint8_t packetBytesList[IBUS_PACKET_BYTES_COUNT]; - packetBytesList[0] = 0x20; - packetBytesList[1] = 0x40; - uint_fast16_t checksum = 0xFFFF - 0x20 - 0x40; - for (size_t i = 2; i < (IBUS_CHANNELS_COUNT * 2) + 2; i++) - { - packetBytesList[i] = this->controlValuesList[i - 2]; - checksum -= packetBytesList[i]; - } - packetBytesList[IBUS_PACKET_BYTES_COUNT - 2] = lowByte(checksum); - packetBytesList[IBUS_PACKET_BYTES_COUNT - 1] = highByte(checksum); - return packetBytesList; -} - -void Ibus::sendPacket() -{ - if (this->isEnabled) - { - uint8_t *packetBytesList = this->createPacket(); - for (size_t i = 0; i < IBUS_PACKET_BYTES_COUNT; i++) - { - this->serial->write(packetBytesList[i]); - } - } -} - -void Ibus::enable() { this->isEnabled = true; } - -void Ibus::disable() { this->isEnabled = false; } - -void Ibus::setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2]) -{ - for (size_t i = 0; i < (IBUS_CHANNELS_COUNT * 2); i++) - { - this->controlValuesList[i] = list[i]; - } -} - -void Ibus::setControlValue(uint8_t channel, uint8_t value) -{ - this->controlValuesList[channel] = value; -} - -bool Ibus::unpackIbusData(uint8_t *packet) -{ - // Verify start and length bytes - if (packet[0] != 0x20 || packet[1] != 0x40) - { - return false; // Invalid packet - } - - // Calculate checksum - uint_fast16_t checksum = 0xFFFF; - for (int i = 0; i < IBUS_PACKET_BYTES_COUNT - 2; i++) - { - checksum -= packet[i]; - } - - // Verify checksum - uint_fast16_t receivedChecksum = - packet[IBUS_PACKET_BYTES_COUNT - 2] | (packet[IBUS_PACKET_BYTES_COUNT - 1] << 8); - if (checksum != receivedChecksum) - { - return false; // Checksum mismatch - } - - // Extract channel values - for (int i = 0; i < IBUS_CHANNELS_COUNT; i++) - { - this->controlValuesList[i] = packet[2 + i * 2] | (packet[3 + i * 2] << 8); - } - - return true; // Successfully unpacked -} - -void Ibus::readLoop() -{ - uint8_t ibusPacket[IBUS_PACKET_BYTES_COUNT]; - int packetIndex = 0; - while (this->serial->available()) - { - uint8_t byte = this->serial->read(); - - // Store byte in packet buffer - ibusPacket[packetIndex++] = byte; - - // Check if we have a full packet - if (packetIndex == IBUS_PACKET_BYTES_COUNT) - { - if (unpackIbusData(ibusPacket)) - { - Serial.println("iBUS Data Unpacked:"); - for (int i = 0; i < IBUS_CHANNELS_COUNT; i++) - { - Serial.print("Channel "); - Serial.print(i + 1); - Serial.print(": "); - Serial.println(this->controlValuesList[i]); - } - } - else - { - Serial.println("Failed to unpack iBUS data."); - } - packetIndex = 0; // Reset for next packet - } - } -} diff --git a/radio/iBUS.cpp b/radio/iBUS.cpp deleted file mode 100644 index 1a536ab..0000000 --- a/radio/iBUS.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// Ibus.cpp -#include "Ibus.h" -#include - -void Ibus2::begin(HardwareSerial &serial, int txPin) -{ - this->serial = &serial; - this->serial->begin(IBUS_BAUD_RATE, SERIAL_8N1, -1, txPin); // Set custom TX pin -} - -void Ibus2::createPacket(uint16_t commands[IBUS_CHANNELS_COUNT]) -{ - packet[0] = 0x20; // Start byte - packet[1] = 0x40; // Length byte - uint_fast16_t checksum = 0xFFFF - 0x20 - 0x40; - for (int i = 0; i < IBUS_CHANNELS_COUNT; i++) - { - packet[2 + i * 2] = commands[i] & 0xFF; // Low byte - packet[3 + i * 2] = (commands[i] >> 8) & 0xFF; // High byte - checksum -= packet[2 + i * 2]; - checksum -= packet[3 + i * 2]; - } - packet[IBUS_PACKET_BYTES_COUNT - 2] = lowByte(checksum); - packet[IBUS_PACKET_BYTES_COUNT - 1] = highByte(checksum); -} - -void Ibus2::sendCommands(uint16_t commands[IBUS_CHANNELS_COUNT]) -{ - createPacket(commands); - for (int i = 0; i < IBUS_PACKET_BYTES_COUNT; i++) - { - this->serial->write(packet[i]); - } -} diff --git a/radio/iBUS.h b/radio/iBUS.h index 3a365a9..c70e91a 100644 --- a/radio/iBUS.h +++ b/radio/iBUS.h @@ -1,23 +1,161 @@ -// Ibus.h -#ifndef IBUS_H -#define IBUS_H - +#include #include - +// Define constants for iBUS #define IBUS_BAUD_RATE 115200 +#define IBUS_SEND_INTERVAL_MS 20 #define IBUS_CHANNELS_COUNT 14 #define IBUS_PACKET_BYTES_COUNT ((IBUS_CHANNELS_COUNT * 2) + 4) -class Ibus2 +// Define the custom TX pin +#define CUSTOM_TX_PIN 17 // Change this to your desired TX pin + +// Define the Ibus class +class Ibus { public: void begin(HardwareSerial &serial, int txPin); - void sendCommands(uint16_t commands[IBUS_CHANNELS_COUNT]); + void loop(); + void enable(); + void disable(); + void readLoop(); + void sendPacket(); + bool unpackIbusData(uint8_t *packet); + void setControlValue(uint8_t channel, uint8_t value); + void setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2]); private: HardwareSerial *serial; - uint8_t packet[IBUS_PACKET_BYTES_COUNT]; - void createPacket(uint16_t commands[IBUS_CHANNELS_COUNT]); + uint8_t controlValuesList[IBUS_CHANNELS_COUNT * 2] = {0}; + bool isEnabled = false; + unsigned long previousMillis = 0; + unsigned long currentMillis = 0; + + uint8_t *createPacket(); }; -#endif // IBUS_H +// Implement the Ibus methods +void Ibus::begin(HardwareSerial &serial, int txPin) +{ + this->serial = &serial; + this->serial->begin(IBUS_BAUD_RATE, SERIAL_8N1, -1, txPin); // Set custom TX pin +} + +void Ibus::loop() +{ + if (this->isEnabled) + { + this->sendPacket(); + // this->readLoop(); + } +} + +uint8_t *Ibus::createPacket() +{ + static uint8_t packetBytesList[IBUS_PACKET_BYTES_COUNT]; + packetBytesList[0] = 0x20; + packetBytesList[1] = 0x40; + uint_fast16_t checksum = 0xFFFF - 0x20 - 0x40; + for (size_t i = 2; i < (IBUS_CHANNELS_COUNT * 2) + 2; i++) + { + packetBytesList[i] = this->controlValuesList[i - 2]; + checksum -= packetBytesList[i]; + } + packetBytesList[IBUS_PACKET_BYTES_COUNT - 2] = lowByte(checksum); + packetBytesList[IBUS_PACKET_BYTES_COUNT - 1] = highByte(checksum); + return packetBytesList; +} + +void Ibus::sendPacket() +{ + if (this->isEnabled) + { + uint8_t *packetBytesList = this->createPacket(); + for (size_t i = 0; i < IBUS_PACKET_BYTES_COUNT; i++) + { + this->serial->write(packetBytesList[i]); + } + } +} + +void Ibus::enable() { this->isEnabled = true; } + +void Ibus::disable() { this->isEnabled = false; } + +void Ibus::setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2]) +{ + for (size_t i = 0; i < (IBUS_CHANNELS_COUNT * 2); i++) + { + this->controlValuesList[i] = list[i]; + } +} + +void Ibus::setControlValue(uint8_t channel, uint8_t value) +{ + this->controlValuesList[channel] = value; +} + +bool Ibus::unpackIbusData(uint8_t *packet) +{ + // Verify start and length bytes + if (packet[0] != 0x20 || packet[1] != 0x40) + { + return false; // Invalid packet + } + + // Calculate checksum + uint_fast16_t checksum = 0xFFFF; + for (int i = 0; i < IBUS_PACKET_BYTES_COUNT - 2; i++) + { + checksum -= packet[i]; + } + + // Verify checksum + uint_fast16_t receivedChecksum = + packet[IBUS_PACKET_BYTES_COUNT - 2] | (packet[IBUS_PACKET_BYTES_COUNT - 1] << 8); + if (checksum != receivedChecksum) + { + return false; // Checksum mismatch + } + + // Extract channel values + for (int i = 0; i < IBUS_CHANNELS_COUNT; i++) + { + this->controlValuesList[i] = packet[2 + i * 2] | (packet[3 + i * 2] << 8); + } + + return true; // Successfully unpacked +} + +void Ibus::readLoop() +{ + uint8_t ibusPacket[IBUS_PACKET_BYTES_COUNT]; + int packetIndex = 0; + while (this->serial->available()) + { + uint8_t byte = this->serial->read(); + + // Store byte in packet buffer + ibusPacket[packetIndex++] = byte; + + // Check if we have a full packet + if (packetIndex == IBUS_PACKET_BYTES_COUNT) + { + if (unpackIbusData(ibusPacket)) + { + Serial.println("iBUS Data Unpacked:"); + for (int i = 0; i < IBUS_CHANNELS_COUNT; i++) + { + Serial.print("Channel "); + Serial.print(i + 1); + Serial.print(": "); + Serial.println(this->controlValuesList[i]); + } + } + else + { + Serial.println("Failed to unpack iBUS data."); + } + packetIndex = 0; // Reset for next packet + } + } +} diff --git a/radio/s-bus.h b/radio/radio-protocol.h similarity index 61% rename from radio/s-bus.h rename to radio/radio-protocol.h index cc454bd..408ee92 100644 --- a/radio/s-bus.h +++ b/radio/radio-protocol.h @@ -1,106 +1,11 @@ -#include "RadioCommands.h" -#include -#include -#include -#include -#include -#include +#include +#include -#define TXD1 39 -#define RXD2 40 -// SBUS packet structure -#define SBUS_PACKET_SIZE 25 - -uint16_t channels[16]; -bool failSafe; -bool lostFrame; - -void readSbusData(); -void writeSbusData(uint16_t channels[]); - -// Data structure to hold channel data -bfs::SbusData sbusDataRead; -bfs::SbusData sbusDataWrite; - -bfs::SbusTx sbusWrite(&Serial1, -1, TXD1, true); // Use Serial1 for SBUS transmission -bfs::SbusRx sbusRead(&Serial2, RXD2, -1, true); // Use Serial2 for SBUS reception - -// P:2:15:4:2 -// BP:0010:1111:0100:0010 uint8_t convertTo4Bit(uint16_t value11Bit); int16_t map4BitTo11Bit(uint8_t value4Bit); +uint8_t map11BitTo4Bit(uint16_t value11Bit); -// print command details -uint16_t getCommandValue(Command cmd) -{ - String commandName = - commandToStringMap.count(cmd) ? commandToStringMap[cmd] : "UNKNOWN"; - Serial.println("RC " + commandName + " : " + String(sbusDataRead.ch[cmd])); - return sbusDataRead.ch[cmd]; -} - -void readSbusData() -{ - // Read SBUS data from Serial2 - if (false && sbusRead.Read()) - { - sbusDataRead = sbusRead.data(); - - Serial.println("Received SBUS data:"); - for (int i = 0; i < bfs::SbusData::NUM_CH; i++) - { - Serial.print("Channel "); - Serial.print(i); - Serial.print(": "); - Serial.println(sbusDataRead.ch[i]); - } - Serial.print("FailSafe: "); - Serial.println(sbusDataRead.failsafe); - Serial.print("Lost Frame: "); - Serial.println(sbusDataRead.lost_frame); - } - if (bool test = true) - { - for (int i = 0; i < 16; i++) - { - sbusDataRead.ch[i] = testChannels[i]; - Serial.print("Channel "); - Serial.print(i); - Serial.print(": "); - Serial.println(sbusDataRead.ch[i]); - } - } -} - -void writeSbusData(uint16_t channels[]) -{ - String str = ""; - // Example: Send SBUS data over Serial1 - for (int i = 0; i < bfs::SbusData::NUM_CH; i++) - { - str += (String(channels[i]) + "-"); - sbusDataWrite.ch[i] = channels[i]; // Example data - } - Serial.println(str); - - sbusWrite.data(sbusDataWrite); - sbusWrite.Write(); -} - -void clearSbusData() -{ - // Assuming bfs::SbusData has a member array `ch` and boolean members `failsafe` and - // `lost_frame` - for (int i = 0; i < bfs::SbusData::NUM_CH; i++) - { - sbusDataRead.ch[i] = 1500; - sbusDataWrite.ch[i] = 1500; - } - sbusDataRead.failsafe = false; - sbusDataRead.lost_frame = false; - sbusDataWrite.failsafe = false; - sbusDataWrite.lost_frame = false; -} +uint8_t convertTo4Bit(uint16_t value11Bit) { return map11BitTo4Bit(value11Bit); } uint8_t map11BitTo4Bit(uint16_t value11Bit) { @@ -149,8 +54,6 @@ int16_t map4BitTo11Bit(uint8_t value4Bit) return 1500; // Or handle the error as needed } -uint8_t convertTo4Bit(uint16_t value11Bit) { return map11BitTo4Bit(value11Bit); } - // Test function void testMap11BitTo4Bit() { diff --git a/radio/radio.h b/radio/radio.h new file mode 100644 index 0000000..ec363b5 --- /dev/null +++ b/radio/radio.h @@ -0,0 +1,378 @@ +#include "SSD1306Wire.h" +#include +#include +#include + +void onReceive(); +void onReceiveFlag(void); +void forceRestartLoRa(); + +bool radioIsRX = false; +unsigned long lastPacketTime = 0; +long int packetN = 0; +int packetSave = 0; +bool packetReceived = false; +// Function to send 2-byte LoRa packet OR 4-byte LoRa packet +void sendLoRaRCPacket(uint8_t cmd1, uint8_t val1, uint8_t cmd2, uint8_t val2, + uint8_t cmd3 = 0, uint8_t val3 = 0, uint8_t cmd4 = 0, + uint8_t val4 = 0) +{ + cmd1 &= 0x0F; + val1 &= 0x0F; + cmd2 &= 0x0F; + val2 &= 0x0F; + int packetSize = LORA_DATA_BYTE; + if (cmd3 != 0 && val3 != 0) + { + cmd3 &= 0x0F; + val3 &= 0x0F; + cmd4 &= 0x0F; + val4 &= 0x0F; + packetSize = 4; + } + + Serial.printf("Sending LoRa Packet: CMD1=%d, VAL1=%d, CMD2=%d, VAL2=%d, ", cmd1, val1, + cmd2, val2); + if (packetSize == 4) + { + Serial.printf("CMD3=%d, VAL3=%d, CMD4=%d, VAL4=%d, ", cmd3, val3, cmd4, val4); + } + Serial.println(); + + uint8_t paddedData[packetSize] = {0}; // Initialize with zeros + // P:2:5:4:10 + // 0010:0101:0100:1010 + // RX 11001110 + // 9546 + // 0010:0101:0100:1010 + uint16_t packet = (cmd1 << 12) | (val1 << 8) | (cmd2 << 4) | val2; + uint16_t packet2 = 0; + + if (packetSize == 4) + { + packet2 = (cmd3 << 12) | (val3 << 8) | (cmd4 << 4) | val4; + } + + packetSave = packet; + if (packet2 != 0) + { + packetSave = (static_cast(packet) << 16) | packet2; + } + + uint8_t data[packetSize] = {0}; + data[0] = (packet >> 8) & 0xFF; // High byte + data[1] = packet & 0xFF; // Low byte + + if (packetSize == 4) + { + data[2] = (packet2 >> 8) & 0xFF; // High byte + data[3] = packet2 & 0xFF; // Low byte + } + + int len = sizeof(data); + + Serial.println("Packet length: " + String(len)); + Serial.print("Sending Packet: "); + Serial.print(data[0], BIN); + Serial.print(" "); + Serial.print(data[1], BIN); + if (packetSize == 4) + { + Serial.print(" "); + Serial.print(data[2], BIN); + Serial.print(" "); + Serial.print(data[3], BIN); + } + Serial.println(); + + // size_t dataSize = sizeof(data) / sizeof(data[0]); + // memcpy(paddedData, data, min(dataSize, (size_t)LORA_DATA_BYTE)); + + int status = radio.transmit(data, sizeof(data)); + if (status == RADIOLIB_ERR_NONE) + { + Serial.println("LoRa Packet Sent!"); + } + else + { + Serial.println("LoRa Transmission Failed."); + } +} + +void sendLoRaPacket(uint8_t *packetData, int length) +{ + // Size of pointer here not an array count. + // int length = sizeof(packetData); + String packetStr = ""; + + Serial.println("Packet length: " + String(length)); + + Serial.print("Sending Packet: "); + for (size_t i = 0; i < length; i++) + { + Serial.print(packetData[i], BIN); + Serial.print(" "); + packetStr += String(packetData[i]); + } + Serial.println(); + display.println("P[" + String(length) + "]:" + packetStr); + Serial.println("P[" + String(length) + "]:" + packetStr); + + int status = radio.transmit(packetData, length); + if (status == RADIOLIB_ERR_NONE) + { + Serial.println("LoRa Packet Sent!"); + } + else + { + Serial.println("LoRa Transmission Failed."); + } +} + +void onReceiveFlag(void) { packetReceived = true; } + +void onReceive(void) +{ +#if LORA_RX + receivedPacketCounter++; + size_t len = radio.getPacketLength(true); + uint8_t data[len] = {0}; + Serial.println("[LoRa] onReceive(" + String(len) + ")"); + +#if DEBUG_RX + Serial.println("[LoRa] onReceive"); + if (len > LORA_DATA_BYTE) + { + Serial.println("WARNING: Packet size is too large:" + String(len)); + } + + Serial.println("[LoRa] Length: " + String(len)); +#endif + int state = radio.readData(data, len); + + if (state == RADIOLIB_ERR_NONE) + { +#if DEBUG_RX + if (sizeof(data) != LORA_DATA_BYTE) + { + Serial.println("[LoRa] ERROR: Packet Length Mismatch!"); + } +#endif + // Packet size 4 processing + if (len == 8) + { + uint64_t seqNum = (static_cast(data[0]) << 56) | + (static_cast(data[1]) << 48) | + (static_cast(data[2]) << 40) | + (static_cast(data[3]) << 32) | + (static_cast(data[4]) << 24) | + (static_cast(data[5]) << 16) | + (static_cast(data[6]) << 8) | + static_cast(data[7]); +#if DEBUG + + Serial.println("Lost:" + String(seqNum) + "/" + + String(receivedPacketCounter) + ":" + + String(seqNum - receivedPacketCounter)); +#endif + int lost = seqNum - receivedPacketCounter; + display.println("Lost:" + String(seqNum) + "/" + + String(receivedPacketCounter) + ":" + String(lost)); + } + + // Check if data is actually zero + else if (data[0] == 0 && data[1] == 0) + { + int length = len; +#if DEBUG + Serial.println("[LoRa] WARNING: Received 0 : 0. I don't know why"); + Serial.print("[LoRa Receiver] Packet length: "); + + Serial.println(String(length)); + + // return; // Ignore this packet + Serial.print("[LoRa Receiver] RSSI: "); + + Serial.print(radio.getRSSI()); + // display.println("0-0"); + + Serial.print("[LoRa Receiver] SNR: "); + Serial.print(radio.getSNR()); + Serial.println(" dB"); +#endif + } + else if (len == 2 || len == 4) //** ToDo: process length 4 */) + { + int length = len; +#if DEBUG + Serial.println("[LoRa Receiver] Packet Received!"); + Serial.print("[LoRa Receiver] Packet length: "); + Serial.println(String(length)); + Serial.println("[LoRa Receiver] DATA: " + String(data[0]) + ":" + + String(data[1])); +#endif + display.println("DATA[" + String(packetN) + "]: " + String(data[0]) + ":" + + String(data[1])); + if (len == 4) + { + display.println("DATA-4[" + String(packetN) + "]: " + String(data[2]) + + ":" + String(data[3])); + } + packetN++; + // Decode received data + uint16_t receivedPacket = (data[0] << 8) | data[1]; + uint8_t cmd1 = (receivedPacket >> 12) & 0x0F; + uint8_t val1 = (receivedPacket >> 8) & 0x0F; + uint8_t cmd2 = (receivedPacket >> 4) & 0x0F; + uint8_t val2 = receivedPacket & 0x0F; + + display.println("Data:" + String(cmd1) + ":" + String(val1) + ":" + + String(cmd2) + ":" + (val2)); + + // If size is 4, decode another 16 bits + uint8_t cmd3 = 0, val3 = 0, cmd4 = 0, val4 = 0; + + if (len == 4) + { + uint16_t receivedPacket2 = (data[2] << 8) | data[3]; + uint8_t cmd3 = (receivedPacket2 >> 12) & 0x0F; + uint8_t val3 = (receivedPacket2 >> 8) & 0x0F; + uint8_t cmd4 = (receivedPacket2 >> 4) & 0x0F; + uint8_t val4 = receivedPacket2 & 0x0F; + + display.println("Data:" + String(cmd3) + ":" + String(val3) + ":" + + String(cmd4) + ":" + (val4)); + } + +#if DEBUG + // Print received data + Serial.print("[LoRa Receiver] Data: "); + Serial.print("CMD1="); + Serial.print(cmd1); + Serial.print(", VAL1="); + Serial.print(val1); + Serial.print(", CMD2="); + Serial.print(cmd2); + Serial.print(", VAL2="); + Serial.println(val2); + Serial.print("[LoRa Receiver] Data: "); + + // Print RSSI (Signal Strength) + Serial.print("[LoRa Receiver] RSSI: "); + Serial.print(radio.getRSSI()); + Serial.println(" dBm"); + + // Print SNR (Signal-to-Noise Ratio) + Serial.print("[LoRa Receiver] SNR: "); + Serial.print(radio.getSNR()); + Serial.println(" dB"); +#endif + display.print("RSSI: " + String(radio.getRSSI())); + display.println(" SNR: " + String(radio.getSNR())); + } + } + else if (state == RADIOLIB_ERR_RX_TIMEOUT) + { + // No packet received + Serial.println("[LoRa Receiver] No packet received."); + } + else if (state == RADIOLIB_ERR_CRC_MISMATCH) + { + // Packet received but corrupted + Serial.println("[LoRa Receiver] Packet received but CRC mismatch!"); + } + else + { + // Other error + Serial.print("[LoRa Receiver] Receive failed, error code: "); + Serial.println(state); + } + // Restart LoRa receiver + // radio.implicitHeader(LORA_DATA_BYTE); + radio.startReceive(); +#endif +} + +void forceRestartLoRa() +{ + Serial.println("[LoRa] Forcing Restart..."); + radio.standby(); + radio.reset(); + delay(100); + radio.begin(); + radio.startReceive(); + lastPacketTime = millis(); // Reset timeout +} + +void initRadioSwitchTable() +{ +#ifdef USING_LR1121 // USING_SX1262 + // LR1121 + // set RF switch configuration for Wio WM1110 + // Wio WM1110 uses DIO5 and DIO6 for RF switching + static const uint32_t rfswitch_dio_pins[] = {RADIOLIB_LR11X0_DIO5, + RADIOLIB_LR11X0_DIO6, RADIOLIB_NC, + RADIOLIB_NC, RADIOLIB_NC}; + + static const Module::RfSwitchMode_t rfswitch_table[] = { + // mode DIO5 DIO6 + {LR11x0::MODE_STBY, {LOW, LOW}}, {LR11x0::MODE_RX, {HIGH, LOW}}, + {LR11x0::MODE_TX, {LOW, HIGH}}, {LR11x0::MODE_TX_HP, {LOW, HIGH}}, + {LR11x0::MODE_TX_HF, {LOW, LOW}}, {LR11x0::MODE_GNSS, {LOW, LOW}}, + {LR11x0::MODE_WIFI, {LOW, LOW}}, END_OF_MODE_TABLE, + }; + radio.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table); + + // LR1121 TCXO Voltage 2.85~3.15V + radio.setTCXO(3.0); + heltec_delay(500); +#endif +} + +void setupRadio() +{ + radio.setFrequency(LORA_BASE_FREQ); + radio.setBandwidth(LORA_BW); + radio.setSpreadingFactor(LORA_SF); +#if LORA_HEADER + // Some issue it receives 0 0 + radio.implicitHeader(LORA_DATA_BYTE); +#else + radio.explicitHeader(); +#endif + radio.setCodingRate(LORA_CR); + radio.setPreambleLength(LORA_PREAMBLE); + radio.forceLDRO(true); + radio.setCRC(2); +#if LORA_TX + radio.setOutputPower(22); + display.println("Turn ON RX to pair you have 30 seconds"); + for (int t = 0; t < 30; t++) + { + display.print("."); + delay(50); + } + display.println(); +#endif + + initRadioSwitchTable(); + +#if LORA_RX + radio.setPacketReceivedAction(onReceiveFlag); + int state = radio.startReceive(); + if (state == RADIOLIB_ERR_NONE) + { + radioIsRX = true; + Serial.println("Listening for LoRa Packets..."); + } + else + { + Serial.print("Receiver failed to start, code: "); + Serial.println(state); + while (true) + { + delay(5); + } + } +#endif +} diff --git a/radio/utility.h b/radio/utility.h new file mode 100644 index 0000000..e966d91 --- /dev/null +++ b/radio/utility.h @@ -0,0 +1,22 @@ +#include + +String toBinary(int num, int bitSize) +{ + if (num == 0) + return "0"; + + String binary = ""; + while (num > 0) + { + binary = String(num % 2) + binary; + num /= 2; + } + + // Pad with leading zeros to match `bitSize` + while (binary.length() < bitSize) + { + binary = "0" + binary; + } + + return binary; +}