mirror of
https://github.com/Genaker/LoraSA.git
synced 2026-03-28 17:42:59 +01:00
558 lines
17 KiB
C++
558 lines
17 KiB
C++
#include <Arduino.h>
|
|
#include <ELRS_GENERIC_LR1221_DIVERSITY.h>
|
|
#include <HardwareSerial.h>
|
|
#include <vector>
|
|
|
|
// Direct access to the low-level SPI communication between RadioLib and the radio module.
|
|
#define RADIOLIB_LOW_LEVEL (1)
|
|
// In this mode, all methods and member variables of all RadioLib classes will be made
|
|
// public and so will be exposed to the user. This allows direct manipulation of the
|
|
// library internals.
|
|
#define RADIOLIB_GODMODE (1)
|
|
#define RADIOLIB_CHECK_PARAMS (0)
|
|
|
|
#include <RadioLib.h>
|
|
#include <SPI.h>
|
|
// #include <SPIEx.h>
|
|
#include <Wire.h>
|
|
|
|
SPIClass hspi = SPIClass(HSPI);
|
|
|
|
#ifdef USING_LR1121
|
|
// Default SPI on pins from pins_arduino.h
|
|
#define RADIO_TYPE LR1121
|
|
#define RADIO_MODULE_INIT() \
|
|
new Module(RADIO_NSS, RADIO_DIO1, RADIO_RST, RADIO_BUSY, hspi);
|
|
|
|
#define RADIO_MODULE2_INIT() \
|
|
new Module(RADIO_NSS_2, RADIO_DIO1_2, RADIO_RST_2, RADIO_BUSY_2, hspi);
|
|
#endif // end USING_LR1121
|
|
|
|
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)
|
|
SPIExClass SPIEx(FSPI);
|
|
#elif defined(PLATFORM_ESP32)
|
|
SPIExClass SPIEx(VSPI);
|
|
#else
|
|
// 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);
|
|
bool parseSetCommand(const String &command, int &startFreq, int &endFreq);
|
|
bool parseGetCommand(const String &command, int &freq);
|
|
|
|
String uartGetResponse();
|
|
std::vector<RSSI> 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<RSSI> rssis);
|
|
void SetDioAsRfSwitch();
|
|
void initRadio(float freq);
|
|
String wrapResult(std::vector<RSSI> 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
|
|
{
|
|
// pinMode(0, INPUT_PULLUP); // Trying to enable auto boot mode. Notes it makes
|
|
// Boot mode always.
|
|
// TODO: make on button press without reset. or with reset :0
|
|
pinMode(RADIO_BUSY, INPUT);
|
|
pinMode(RADIO_DIO1, INPUT);
|
|
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);
|
|
SPI.setHwCs(true);
|
|
radio = RADIO_MODULE_INIT();
|
|
radio2 = RADIO_MODULE2_INIT();
|
|
|
|
// Initialize Serial Monitor
|
|
Serial.begin(115200);
|
|
delay(1000);
|
|
Serial.println("ELRS TRUE DIVERSITY LR1121 ESP32-PICO-D4!");
|
|
float freq = 900;
|
|
initRadio(freq);
|
|
delay(1000);
|
|
}
|
|
|
|
float freq = 1200;
|
|
// Scan Freq in KH
|
|
int startFreq = 0, endFreq = 0;
|
|
int getFreq = 0;
|
|
void loop()
|
|
{
|
|
std::vector<RSSI> rssis;
|
|
// getFreq = 0;
|
|
if (Serial.available() > 0)
|
|
{
|
|
String command = Serial.readStringUntil('\n'); // Read until newline
|
|
command.trim(); // Remove any leading/trailing whitespace
|
|
|
|
// Process the command
|
|
processCommands(command);
|
|
}
|
|
if (getFreq != 0)
|
|
{
|
|
freq = (float)getFreq; /// 1000.0f;
|
|
float rssi = 0, rssi2 = 0;
|
|
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)
|
|
{
|
|
rssis = getRssiRange(startFreq, endFreq, 1, 1);
|
|
String rssiString = wrapResult(rssis); // printRssiRange(rssis);
|
|
const char *RSSIutf8Data = rssiString.c_str();
|
|
Serial.println(RSSIutf8Data);
|
|
}
|
|
}
|
|
|
|
std::vector<RSSI> getRssiRange(int startFreq, int endFreq, float step, float samples,
|
|
int group)
|
|
{
|
|
long int startTime = millis();
|
|
std::vector<RSSI> 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));
|
|
state = radio.setFrequency((float)freq);
|
|
state2 = radio2.setFrequency((float)freq);
|
|
usleep(100);
|
|
if (state != RADIOLIB_ERR_NONE)
|
|
{
|
|
Serial.println("Error:setFrequency:" + String(state));
|
|
}
|
|
if (state2 != RADIOLIB_ERR_NONE)
|
|
{
|
|
Serial.println("Error2:setFrequency:" + String(state2));
|
|
}
|
|
|
|
#if defined(USING_LR1121)
|
|
// Try getRssiInst
|
|
radio.getRssiInst(&rssi);
|
|
radio2.getRssiInst(&rssi2);
|
|
usleep(10);
|
|
// Serial.println("RSSI: " + String(rssi));
|
|
// pass the replies
|
|
#endif
|
|
}
|
|
|
|
// Function to process commands
|
|
void processCommands(const String &command)
|
|
{
|
|
if (isSetCommand(command))
|
|
{
|
|
// Global freq range
|
|
startFreq = 0, endFreq = 0;
|
|
if (parseSetCommand(command, startFreq, endFreq))
|
|
{
|
|
Serial.print("Parsed successfully: Start = ");
|
|
Serial.print(startFreq);
|
|
Serial.print(", End = ");
|
|
Serial.println(endFreq);
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Invalid SET command format!");
|
|
}
|
|
}
|
|
else if (isGetCommand(command))
|
|
{
|
|
int freq;
|
|
if (parseGetCommand(command, freq))
|
|
{
|
|
Serial.print("Parsed successfully: Freq = ");
|
|
Serial.println(freq);
|
|
getFreq = freq;
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Invalid GET command format!");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Unknown command received!");
|
|
}
|
|
}
|
|
|
|
// 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('-');
|
|
int spaceIndex = command.indexOf(' ');
|
|
|
|
if (spaceIndex == -1 || dashIndex == -1 || dashIndex < spaceIndex)
|
|
return false;
|
|
|
|
// Extract the two parts
|
|
String startStr = command.substring(spaceIndex + 1, dashIndex);
|
|
String endStr = command.substring(dashIndex + 1);
|
|
|
|
// Convert to integers
|
|
startFreq = startStr.toInt();
|
|
endFreq = endStr.toInt();
|
|
|
|
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(' ');
|
|
|
|
if (spaceIndex == -1) // Ensure there's a space in the command
|
|
return false;
|
|
|
|
// Extract the frequency part
|
|
String freqStr = command.substring(spaceIndex + 1);
|
|
|
|
// Convert to a long integer
|
|
freq = freqStr.toInt();
|
|
|
|
// Validate the conversion
|
|
if (freq <= 0) // Invalid conversion or value
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
String uartGetResponse()
|
|
{
|
|
String response = "";
|
|
unsigned long startTime = millis();
|
|
while (millis() - startTime < 1000)
|
|
{ // Wait for up to 1 second
|
|
while (Serial1.available())
|
|
{ // Check if data is available
|
|
char c = Serial1.read(); // Read one character
|
|
if (c == '\n')
|
|
{ // Check for end of response (newline)
|
|
return response;
|
|
}
|
|
response += c; // Append character to response
|
|
}
|
|
}
|
|
return ""; // Return empty string if no response
|
|
}
|
|
|
|
String printRssiRange(std::vector<RSSI> 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;
|
|
radio.begin();
|
|
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<RSSI> 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
|
|
}
|
|
}
|
|
}
|