From 18b95c75a80ae1e479967ae6ac7baa51ae35b465 Mon Sep 17 00:00:00 2001 From: Egor Date: Thu, 23 Jan 2025 21:29:20 -0800 Subject: [PATCH] ELRS LR1121 direction finding --- elrs-boards/main.cpp | 411 +++++++++++++++++++----- include/ELRS_GENERIC_LR1221_DIVERSITY.h | 16 + 2 files changed, 345 insertions(+), 82 deletions(-) diff --git a/elrs-boards/main.cpp b/elrs-boards/main.cpp index b8d313e..9690d31 100644 --- a/elrs-boards/main.cpp +++ b/elrs-boards/main.cpp @@ -1,6 +1,7 @@ #include #include #include +#include // Direct access to the low-level SPI communication between RadioLib and the radio module. #define RADIOLIB_LOW_LEVEL (1) @@ -30,6 +31,9 @@ SPIClass hspi = SPIClass(HSPI); RADIO_TYPE radio = RADIO_MODULE_INIT(); RADIO_TYPE radio2 = RADIO_MODULE2_INIT(); +int radioIsHigh = false; +int radio2IsHigh = false; + // SPIClass hspi = new SPIClass(2); #if defined(PLATFORM_ESP32_S3) || defined(PLATFORM_ESP32_C3) @@ -40,6 +44,15 @@ SPIExClass SPIEx(VSPI); // SPIExClass SPIEx; #endif // +// Define a struct to represent a single RSSI scan +struct RSSI +{ + float freq; // Frequency in MHz + float rssi1; // RSSI value 1 + float rssi2; // RSSI value 2 + // float rssi3; // RSSI value 3 (optional, can add more if needed) +}; + bool isSetCommand(const String &command); bool isGetCommand(const String &command); void processCommands(const String &command); @@ -47,8 +60,28 @@ bool parseSetCommand(const String &command, int &startFreq, int &endFreq); bool parseGetCommand(const String &command, int &freq); String uartGetResponse(); +std::vector getRssiRange(int startFreq, int endFreq, float step = 1, + float samples = 1, int group = 1); void getRssi(float freq, float &rssi, float &rssi2); +String printRssiRange(std::vector rssis); +void SetDioAsRfSwitch(); +void initRadio(float freq); +String wrapResult(std::vector rssis, bool single = false); + +// Function to check if the command starts with "SET" +bool isSetCommand(const String &command) { return command.startsWith("SET "); } + +// Function to check if the command starts with "GET" +bool isGetCommand(const String &command) { return command.startsWith("GET "); } + +enum class Direction +{ + Left, + Right +}; + +String stringToMinLength(String input, int minLength); void setup() // https://github.com/ExpressLRS/ExpressLRS/blob/2e61e33723b3247300f95ed46ceee9648d536137/src/lib/LR1121Driver/LR1121_hal.cpp#L6 @@ -61,6 +94,11 @@ void setup() pinMode(RADIO_NSS, OUTPUT); digitalWrite(RADIO_NSS, HIGH); + pinMode(RADIO_BUSY_2, INPUT); + pinMode(RADIO_DIO1_2, INPUT); + pinMode(RADIO_NSS_2, OUTPUT); + digitalWrite(RADIO_NSS_2, HIGH); + hspi.begin(RADIO_SCK, RADIO_MISO, RADIO_MOSI, RADIO_NSS); gpio_pullup_en((gpio_num_t)RADIO_MISO); SPI.setFrequency(17500000); @@ -72,54 +110,8 @@ void setup() Serial.begin(115200); delay(1000); Serial.println("ELRS TRUE DIVERSITY LR1121 ESP32-PICO-D4!"); - int state, state2; float freq = 900; - state = radio.beginGFSK(freq, 4.8F, 5.0F, 156.2F, 10, 16U, 1.6F); - state2 = radio2.beginGFSK(freq, 4.8F, 5.0F, 156.2F, 10, 16U, 1.6F); - if (state != RADIOLIB_ERR_NONE) - { - Serial.println("Error:beginGFSK" + String(state)); - } - if (state2 != RADIOLIB_ERR_NONE) - { - Serial.println("Error2:beginGFSK" + String(state2)); - } - // RF Switch info Provided by LilyGo support: - // https://github.com/Xinyuan-LilyGO/LilyGo-LoRa-Series/blob/f2d3d995cba03c65a7031c73e212f106b03c95a2/examples/RadioLibExamples/Receive_Interrupt/Receive_Interrupt.ino#L279 - - // 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); - radio2.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table); - - // LR1121 TCXO Voltage 2.85~3.15V - radio.setTCXO(3.0); - radio2.setTCXO(3.0); - state = radio.setDataShaping(RADIOLIB_SHAPING_NONE); - state = radio.startReceive(RADIOLIB_LR11X0_RX_TIMEOUT_NONE); - state2 = radio2.setDataShaping(RADIOLIB_SHAPING_NONE); - state2 = radio2.startReceive(RADIOLIB_LR11X0_RX_TIMEOUT_NONE); - if (state != RADIOLIB_ERR_NONE) - { - Serial.println("Error:startReceive:" + String(state)); - } - if (state2 != RADIOLIB_ERR_NONE) - { - Serial.println("Error2:startReceive:" + String(state2)); - } - + initRadio(freq); delay(1000); } @@ -129,7 +121,8 @@ int startFreq = 0, endFreq = 0; int getFreq = 0; void loop() { - getFreq = 0; + std::vector rssis; + // getFreq = 0; if (Serial.available() > 0) { String command = Serial.readStringUntil('\n'); // Read until newline @@ -140,45 +133,126 @@ void loop() } if (getFreq != 0) { - freq = (float)getFreq / 1000.0f; + freq = (float)getFreq; /// 1000.0f; float rssi = 0, rssi2 = 0; - getRssi(freq, rssi, rssi2); - Serial.println("[" + String(freq) + "Mhz]RSSI: " + String((float)rssi) + + rssis = getRssiRange(freq, freq, 0.5, 1); + // getRssi(freq, rssi, rssi2); + rssi = rssis[0].rssi1; + rssi2 = rssis[0].rssi2; + Serial.println("[" + String(freq / 1000) + "Mhz]RSSI: " + String((float)rssi) + " RSSI2: " + String((float)rssi2)); Serial.println("RSSI1: " + String((float)rssi) + " RSSI2: " + String((float)rssi2)); + int delta = 0; + if ((int)rssi > (int)rssi2) + { + String arrow = ""; + delta = abs(abs(rssi) - abs(rssi2)); + for (int i = 0; i < 4; i++) + { + arrow += ">"; + } + // arrow = stringToMinLength(arrow, 5); + Serial.println(" " + String((int)rssi) + "[" + String((int)freq / 1000) + + "]" + String((int)rssi2) + arrow); + } + else if ((int)rssi < (int)rssi2) + { + delta = abs(abs(rssi) - abs(rssi2)); + String arrow = ""; + for (int i = 0; i < 4; i++) + { + arrow += "<"; + } + // arrow = stringToMinLength(arrow, -5); + Serial.println(arrow + String((int)rssi) + "[" + String((int)freq / 1000) + + "]" + String((int)rssi2) + " "); + } + else if (abs((int)rssi2) - abs((int)rssi2) == 0) + { + Serial.println(" " + String((int)rssi) + "[" + String((int)freq / 1000) + + "]" + String((int)rssi2) + " "); + } + // String rssiString = wrapResult(rssis); // printRssiRange(rssis); + // const char *RSSIutf8Data = rssiString.c_str(); + // Serial.println(RSSIutf8Data); + delay(1000); } if (startFreq != 0 && endFreq != 0) { - if (startFreq > endFreq) - { - float temp = startFreq; - startFreq = endFreq; - endFreq = temp; - } - float step = 1000; // Step size in MHz - for (float freq = startFreq; freq <= endFreq; freq += step) - { - float rssi = 0, rssi2 = 0; - getRssi((float)freq / 1000.0f, rssi, rssi2); // Call the method to get RSSI - Serial.print("Frequency: "); - Serial.print(freq, 1); // Print frequency with 1 decimal place - Serial.print(" KHz, RSSI: "); - Serial.print(rssi); - Serial.print(" RSSI2: "); - Serial.println(rssi2); - } + rssis = getRssiRange(startFreq, endFreq, 1, 1); + String rssiString = wrapResult(rssis); // printRssiRange(rssis); + const char *RSSIutf8Data = rssiString.c_str(); + Serial.println(RSSIutf8Data); } - delay(1); +} + +std::vector getRssiRange(int startFreq, int endFreq, float step, float samples, + int group) +{ + long int startTime = millis(); + std::vector rssis; + if (startFreq > endFreq) + { + float temp = startFreq; + startFreq = endFreq; + endFreq = temp; + } + step = step * 1000; // Step size in MHz + RSSI scan; + for (float freq = startFreq; freq <= endFreq; freq += step) + { + float rssi = 0, rssi2 = 0; + float maxRssi = -999, maxRssi2 = -999; + for (int i = 0; i < samples; i++) + { + getRssi((float)freq / 1000.0f, rssi, rssi2); // Call the method to get RSSI + if (maxRssi < rssi) + { + maxRssi = rssi; + } + if (maxRssi2 < rssi2) + { + maxRssi2 = rssi2; + } + } + rssi = maxRssi; + rssi2 = maxRssi2; + /** + Serial.print("Frequency: "); + Serial.print(freq, 1); // Print frequency with 1 decimal place + Serial.print(" KHz, RSSI: "); + Serial.print(rssi); + Serial.print(" RSSI2: "); + Serial.println(rssi2); + **/ + scan = {freq, rssi, rssi2}; + rssis.push_back(scan); + } + long int endTime = millis(); + // Serial.println("Get RSSI TIME: " + String(endTime - startTime)); + return rssis; } void getRssi(float freq, float &rssi, float &rssi2) { + if (freq > 1000 && !radioIsHigh) + { + initRadio(freq); + radioIsHigh = true; + } + if (freq < 1000 && radioIsHigh) + { + initRadio(freq); + radioIsHigh = false; + } + int state, state2; // Print message periodically - Serial.println("Freq: " + String(freq)); + // Serial.println("Freq: " + String(freq)); state = radio.setFrequency((float)freq); state2 = radio2.setFrequency((float)freq); + usleep(100); if (state != RADIOLIB_ERR_NONE) { Serial.println("Error:setFrequency:" + String(state)); @@ -190,9 +264,9 @@ void getRssi(float freq, float &rssi, float &rssi2) #if defined(USING_LR1121) // Try getRssiInst - radio.getRssiInst(&rssi); radio2.getRssiInst(&rssi2); + usleep(10); // Serial.println("RSSI: " + String(rssi)); // pass the replies #endif @@ -237,14 +311,15 @@ void processCommands(const String &command) } } -// Function to check if the command starts with "SET" -bool isSetCommand(const String &command) { return command.startsWith("SET "); } - -// Function to check if the command starts with "GET" -bool isGetCommand(const String &command) { return command.startsWith("GET "); } - // Function to parse the "SET" command // Example: SET 915000-920000 +// Example: SET 915000-916000 +// Example: SET 900000-1000000 +// Example: SET 900000-960000 +// Example: SET 2000000-2500000 +// Example: SET 2410000-2460000 +// Example: SET 2448000-2452000 +// Example: SET 0-0 //off bool parseSetCommand(const String &command, int &startFreq, int &endFreq) { int dashIndex = command.indexOf('-'); @@ -261,14 +336,12 @@ bool parseSetCommand(const String &command, int &startFreq, int &endFreq) startFreq = startStr.toInt(); endFreq = endStr.toInt(); - // Validate the conversion - if (startFreq == 0 || endFreq == 0) - return false; - return true; } // Function to parse the "GET" command with a large frequency value +// GET 915000 +// GET 2450000 bool parseGetCommand(const String &command, int &freq) { int spaceIndex = command.indexOf(' '); @@ -307,3 +380,177 @@ String uartGetResponse() } return ""; // Return empty string if no response } + +String printRssiRange(std::vector rssis) +{ + String result = "SCAN_RESULT "; + for (const auto &rssi : rssis) + { + result += String((int)rssi.freq) + "," + String(rssi.rssi1) + "," + + String(rssi.rssi2) + ";"; + } + if (result == "SCAN_RESULT ") + { + result = ""; + } + return result; +} + +#define LR1121_RFSW_CTRL_COUNT 8; + +void SetDioAsRfSwitch() +{ + // 4.2.1 SetDioAsRfSwitch + uint8_t switchbuf[8]; + /*if (LR1121_RFSW_CTRL_COUNT == 8) + { + switchbuf[0] = LR1121_RFSW_CTRL[0]; // RfswEnable + switchbuf[1] = LR1121_RFSW_CTRL[1]; // RfSwStbyCfg + switchbuf[2] = LR1121_RFSW_CTRL[2]; // RfSwRxCfg + switchbuf[3] = LR1121_RFSW_CTRL[3]; // RfSwTxCfg + switchbuf[4] = LR1121_RFSW_CTRL[4]; // RfSwTxHPCfg + switchbuf[5] = LR1121_RFSW_CTRL[5]; // RfSwTxHfCfg + switchbuf[6] = LR1121_RFSW_CTRL[6]; // Unused + switchbuf[7] = + LR1121_RFSW_CTRL[7]; // RfSwWifiCfg - Each bit indicates the state of the + // relevant RFSW DIO when in Wi-Fi scanning mode or high + // frequency RX mode (LR1110_H1_UM_V1-7-1.pdf) + } + else*/ + { + switchbuf[0] = 0b00001111; // RfswEnable + switchbuf[1] = 0b00000000; // RfSwStbyCfg + switchbuf[2] = 0b00000100; // RfSwRxCfg + switchbuf[3] = 0b00001000; // RfSwTxCfg + switchbuf[4] = 0b00001000; // RfSwTxHPCfg + switchbuf[5] = 0b00000010; // RfSwTxHfCfg + switchbuf[6] = 0; // Unused + switchbuf[7] = 0b00000001; // RfSwWifiCfg - Each bit indicates the state of the + // relevant RFSW DIO when in Wi-Fi scanning mode or + // high frequency RX mode (LR1110_H1_UM_V1-7-1.pdf) + } + radio.SPIcommand(LR11XX_SYSTEM_SET_DIO_AS_RF_SWITCH_OC, true, switchbuf, + sizeof(switchbuf)); + radio2.SPIcommand(LR11XX_SYSTEM_SET_DIO_AS_RF_SWITCH_OC, true, switchbuf, + sizeof(switchbuf)); + + // 4.1.1 SetDioIrqParams + + uint8_t buf[8] = {0}; + buf[3] = LR1121_IRQ_TX_DONE | LR1121_IRQ_RX_DONE; + radio.SPIcommand(LR11XX_SYSTEM_SET_DIOIRQPARAMS_OC, true, buf, sizeof(buf)); + radio2.SPIcommand(LR11XX_SYSTEM_SET_DIOIRQPARAMS_OC, true, buf, sizeof(buf)); + + if (RADIO_DCDC) + { + // 5.1.1 SetRegMode + uint8_t RegMode[1] = {1}; // Enable DCDC converter instead of LDO + radio.SPIcommand(LR11XX_SYSTEM_SET_REGMODE_OC, true, RegMode, sizeof(RegMode)); + radio2.SPIcommand(LR11XX_SYSTEM_SET_REGMODE_OC, true, RegMode, sizeof(RegMode)); + } +} + +void initRadio(float freq) +{ + int state, state2; + state = radio.beginGFSK(freq, 4.8F, 5.0F, 156.2F, 10, 16U, 1.6F); + state2 = radio2.beginGFSK(freq, 4.8F, 5.0F, 156.2F, 10, 16U, 1.6F); + if (state != RADIOLIB_ERR_NONE) + { + Serial.println("Error:beginGFSK" + String(state)); + } + if (state2 != RADIOLIB_ERR_NONE) + { + Serial.println("Error2:beginGFSK" + String(state2)); + } + // RF Switch info Provided by LilyGo support: + // https://github.com/Xinyuan-LilyGO/LilyGo-LoRa-Series/blob/f2d3d995cba03c65a7031c73e212f106b03c95a2/examples/RadioLibExamples/Receive_Interrupt/Receive_Interrupt.ino#L279 + + // 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); + radio2.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table); + + // SetDioAsRfSwitch(); + // LR1121 TCXO Voltage 2.85~3.15V + radio.setTCXO(3.0); + radio2.setTCXO(3.0); + state = radio.setDataShaping(RADIOLIB_SHAPING_NONE); + state = radio.startReceive(RADIOLIB_LR11X0_RX_TIMEOUT_NONE); + state2 = radio2.setDataShaping(RADIOLIB_SHAPING_NONE); + state2 = radio2.startReceive(RADIOLIB_LR11X0_RX_TIMEOUT_NONE); + if (state != RADIOLIB_ERR_NONE) + { + Serial.println("Error:startReceive:" + String(state)); + } + if (state2 != RADIOLIB_ERR_NONE) + { + Serial.println("Error2:startReceive:" + String(state2)); + } +} + +String wrapResult(std::vector rssis, bool single) +{ + String wrapped = "SCAN_RESULT " + String(rssis.size()) + " [ "; + + for (const auto &rssi : rssis) + { + //(<частота кГц>, <сила сигналу, dBm>), (<частота кГц>, <сила сигналу, dBm>) + wrapped += " (" + String((int)rssi.freq) + ", " + String(rssi.rssi1); + if (!single) + { + wrapped += ", " + String(rssi.rssi2); + } + + wrapped += "), "; + } + // Remove the last comma, if it exists + if (wrapped.length() > 0) + { + wrapped.remove(wrapped.length() - 1); + wrapped.remove(wrapped.length() - 1); + } + wrapped += "]"; + return wrapped; +} + +String stringToMinLength(String input, int length) +{ + // Determine absolute length for truncation + int absLength = abs(length); + + if (input.length() == absLength) + { + return input; // Exact length, no change needed + } + else if (input.length() > absLength) + { + return input.substring(0, absLength); // Truncate the string to the desired length + } + else + { + int paddingLength = absLength - input.length(); + String padding(paddingLength, ' '); // Create padding of spaces + + if (length > 0) + { + return padding + input; // Left padding + } + else + { + return input + padding; // Right padding + } + } +} diff --git a/include/ELRS_GENERIC_LR1221_DIVERSITY.h b/include/ELRS_GENERIC_LR1221_DIVERSITY.h index 2407043..97c91cb 100644 --- a/include/ELRS_GENERIC_LR1221_DIVERSITY.h +++ b/include/ELRS_GENERIC_LR1221_DIVERSITY.h @@ -17,7 +17,21 @@ #define BUTTON 0 #define LED RGB 22 +#define LR11XX_SYSTEM_SET_DIO_AS_RF_SWITCH_OC 0x0112 +#define LR11XX_SYSTEM_SET_DIOIRQPARAMS_OC 0x0113 +#define LR11XX_SYSTEM_SET_REGMODE_OC 0x0110 +#define SX12XX_Radio_NONE 0b00000000 // Bit mask for no radio +#define SX12XX_Radio_1 0b00000001 // Bit mask for radio 1 +#define SX12XX_Radio_2 0b00000010 // Bit mask for radio 2 +#define SX12XX_Radio_All 0b00000011 // bit mask for both radios +#define LR1121_IRQ_TX_DONE 0x00000004 +#define LR1121_IRQ_RX_DONE 0x00000008 + +#define RADIO_DCDC true + + const int LR1121_RFSW_CTRL[] = {31, 0, 4, 8, 8, 18, 0, 17}; +const int LR1121_RFSW_CTRL_COUNT = sizeof(LR1121_RFSW_CTRL) / sizeof(LR1121_RFSW_CTRL); // Source: // https://github.com/ExpressLRS/targets/blob/4f3b2cba00c706da6a75f545a0650d0b0b0386ee/RX/Generic%20LR1121%20True%20Diversity.json @@ -35,4 +49,6 @@ "radio_dio1_2": 34, "radio_nss_2": 13, "radio_rst_2": 21, + + "radio_dcdc": true, */