diff --git a/data/index.html b/data/index.html
index f5da9fd..ca94cbb 100755
--- a/data/index.html
+++ b/data/index.html
@@ -111,6 +111,10 @@ Phobos LapTimer
+
+
+
+
diff --git a/data/script.js b/data/script.js
index 9e99ac7..3e8b60f 100644
--- a/data/script.js
+++ b/data/script.js
@@ -13,6 +13,7 @@ const ssidInput = document.getElementById("ssid");
const pwdInput = document.getElementById("pwd");
const minLapInput = document.getElementById("minLap");
const alarmThreshold = document.getElementById("alarmThreshold");
+const elrsBindPhrase = document.getElementById("elrsBindPhrase");
const freqLookup = [
[5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725],
@@ -85,6 +86,7 @@ onload = function (e) {
timer.innerHTML = "00:00:00 s";
clearLaps();
createRssiChart();
+ elrsBindPhrase.value = config.elrsBindPhrase
});
};
@@ -245,6 +247,7 @@ function saveConfig() {
name: pilotNameInput.value,
ssid: ssidInput.value,
pwd: pwdInput.value,
+ elrsBindPhrase: elrsBindPhrase.value,
}),
})
.then((response) => response.json())
diff --git a/lib/CONFIG/config.cpp b/lib/CONFIG/config.cpp
index a481808..4432067 100644
--- a/lib/CONFIG/config.cpp
+++ b/lib/CONFIG/config.cpp
@@ -59,6 +59,7 @@ void Config::toJson(AsyncResponseStream& destination) {
config["name"] = conf.pilotName;
config["ssid"] = conf.ssid;
config["pwd"] = conf.password;
+ config["elrsBindPhrase"] = conf.elrsBindPhrase;
serializeJson(config, destination);
}
@@ -74,6 +75,7 @@ void Config::toJsonString(char* buf) {
config["name"] = conf.pilotName;
config["ssid"] = conf.ssid;
config["pwd"] = conf.password;
+ config["elrsBindPhrase"] = conf.elrsBindPhrase;
serializeJsonPretty(config, buf, 256);
}
@@ -118,6 +120,10 @@ void Config::fromJson(JsonObject source) {
strlcpy(conf.password, source["pwd"] | "", sizeof(conf.password));
modified = true;
}
+ if (source["elrsBindPhrase"] != conf.elrsBindPhrase) {
+ strlcpy(conf.elrsBindPhrase, source["elrsBindPhrase"] | "", sizeof(conf.elrsBindPhrase));
+ modified = true;
+ }
}
uint16_t Config::getFrequency() {
@@ -148,6 +154,10 @@ char* Config::getPassword() {
return conf.password;
}
+char* Config::getElrsBindPhrase() {
+ return conf.elrsBindPhrase;
+}
+
void Config::setDefaults(void) {
DEBUG("Setting EEPROM defaults\n");
// Reset everything to 0/false and then just set anything that zero is not appropriate
@@ -163,6 +173,7 @@ void Config::setDefaults(void) {
strlcpy(conf.ssid, "", sizeof(conf.ssid));
strlcpy(conf.password, "", sizeof(conf.password));
strlcpy(conf.pilotName, "", sizeof(conf.pilotName));
+ strlcpy(conf.elrsBindPhrase, "", sizeof(conf.elrsBindPhrase));
modified = true;
write();
}
diff --git a/lib/CONFIG/config.h b/lib/CONFIG/config.h
index e46b834..11c9750 100644
--- a/lib/CONFIG/config.h
+++ b/lib/CONFIG/config.h
@@ -81,6 +81,7 @@ typedef struct {
char pilotName[21];
char ssid[33];
char password[33];
+ char elrsBindPhrase[32];
} laptimer_config_t;
class Config {
@@ -101,6 +102,7 @@ class Config {
uint8_t getExitRssi();
char* getSsid();
char* getPassword();
+ char* getElrsBindPhrase();
private:
laptimer_config_t conf;
diff --git a/lib/LAPTIMER/laptimer.cpp b/lib/LAPTIMER/laptimer.cpp
index 8711233..4d34868 100644
--- a/lib/LAPTIMER/laptimer.cpp
+++ b/lib/LAPTIMER/laptimer.cpp
@@ -10,6 +10,7 @@ void LapTimer::init(Config *config, RX5808 *rx5808, Buzzer *buzzer, Led *l) {
rx = rx5808;
buz = buzzer;
led = l;
+ startTimeOffsetMs = 0;
filter.setMeasurementNoise(rssi_filter_q * 0.01f);
filter.setProcessNoise(rssi_filter_r * 0.0001f);
@@ -21,6 +22,7 @@ void LapTimer::init(Config *config, RX5808 *rx5808, Buzzer *buzzer, Led *l) {
void LapTimer::start() {
DEBUG("LapTimer started\n");
state = RUNNING;
+ startTimeOffsetMs = millis();
buz->beep(500);
led->on(500);
}
@@ -30,6 +32,7 @@ void LapTimer::stop() {
state = STOPPED;
lapCount = 0;
rssiCount = 0;
+ startTimeOffsetMs = 0;
memset(lapTimes, 0, sizeof(lapTimes));
buz->beep(500);
led->on(500);
@@ -75,7 +78,7 @@ void LapTimer::lapPeakCapture() {
// Check if RSSI is greater than the previous detected peak
if (rssi[rssiCount] > rssiPeak) {
rssiPeak = rssi[rssiCount];
- rssiPeakTimeMs = millis();
+ rssiPeakTimeMs = millis() - startTimeOffsetMs;
}
}
}
diff --git a/lib/LAPTIMER/laptimer.h b/lib/LAPTIMER/laptimer.h
index 42a39f9..83c7605 100644
--- a/lib/LAPTIMER/laptimer.h
+++ b/lib/LAPTIMER/laptimer.h
@@ -30,6 +30,7 @@ class LapTimer {
Buzzer *buz;
Led *led;
KalmanFilter filter;
+ uint32_t startTimeOffsetMs;
uint32_t startTimeMs;
uint8_t lapCount;
uint8_t rssiCount;
diff --git a/lib/MSP/Readme.md b/lib/MSP/Readme.md
new file mode 100644
index 0000000..c0d1cb4
--- /dev/null
+++ b/lib/MSP/Readme.md
@@ -0,0 +1 @@
+Files are copied from https://github.com/ExpressLRS/Backpack/tree/master/lib/MSP commit 476d7c5
diff --git a/lib/MSP/msp.cpp b/lib/MSP/msp.cpp
new file mode 100644
index 0000000..a7b2b07
--- /dev/null
+++ b/lib/MSP/msp.cpp
@@ -0,0 +1,314 @@
+#include "msp.h"
+#include "debug.h"
+/* ==========================================
+MSP V2 Message Structure:
+Offset: Usage: In CRC: Comment:
+======= ====== ======= ========
+0 $ Framing magic start char
+1 X 'X' in place of v1 'M'
+2 type '<' / '>' / '!' Message Type (TODO find out what ! type is)
+3 flag + uint8, flag, usage to be defined (set to zero)
+4 function + uint16 (little endian). 0 - 255 is the same function as V1 for backwards compatibility
+6 payload size + uint16 (little endian) payload size in bytes
+8 payload + n (up to 65535 bytes) payload
+n+8 checksum uint8, (n= payload size), crc8_dvb_s2 checksum
+========================================== */
+
+// CRC helper function. External to MSP class
+// TODO: Move all our CRC functions to a CRC lib
+uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
+{
+ crc ^= a;
+ for (int ii = 0; ii < 8; ++ii) {
+ if (crc & 0x80) {
+ crc = (crc << 1) ^ 0xD5;
+ } else {
+ crc = crc << 1;
+ }
+ }
+ return crc;
+}
+
+MSP::MSP() : m_inputState(MSP_IDLE)
+{
+}
+
+bool
+MSP::processReceivedByte(uint8_t c)
+{
+ switch (m_inputState) {
+
+ case MSP_IDLE:
+ // Wait for framing char
+ if (c == '$') {
+ m_inputState = MSP_HEADER_START;
+ }
+ break;
+
+ case MSP_HEADER_START:
+ // Waiting for 'X' (MSPv2 native)
+ switch (c) {
+ case 'X':
+ m_inputState = MSP_HEADER_X;
+ break;
+ default:
+ m_inputState = MSP_IDLE;
+ break;
+ }
+ break;
+
+ case MSP_HEADER_X:
+ // Wait for the packet type (cmd or req)
+ m_inputState = MSP_HEADER_V2_NATIVE;
+
+ // Start of a new packet
+ // reset the packet, offset iterator, and CRC
+ m_packet.reset();
+ m_offset = 0;
+ m_crc = 0;
+
+ switch (c) {
+ case '<':
+ m_packet.type = MSP_PACKET_COMMAND;
+ break;
+ case '>':
+ m_packet.type = MSP_PACKET_RESPONSE;
+ break;
+ default:
+ m_packet.type = MSP_PACKET_UNKNOWN;
+ m_inputState = MSP_IDLE;
+ break;
+ }
+ break;
+
+ case MSP_HEADER_V2_NATIVE:
+ // Read bytes until we have a full header
+ m_inputBuffer[m_offset++] = c;
+ m_crc = crc8_dvb_s2(m_crc, c);
+
+ // If we've received the correct amount of bytes for a full header
+ if (m_offset == sizeof(mspHeaderV2_t)) {
+ // Copy header values into packet
+ mspHeaderV2_t* header = (mspHeaderV2_t*)&m_inputBuffer[0];
+ m_packet.payloadSize = header->payloadSize;
+ m_packet.function = header->function;
+ m_packet.flags = header->flags;
+ // reset the offset iterator for re-use in payload below
+ m_offset = 0;
+ if (m_packet.payloadSize == 0)
+ m_inputState = MSP_CHECKSUM_V2_NATIVE;
+ else
+ m_inputState = MSP_PAYLOAD_V2_NATIVE;
+ }
+ break;
+
+ case MSP_PAYLOAD_V2_NATIVE:
+ // Read bytes until we reach payloadSize
+ m_packet.payload[m_offset++] = c;
+ m_crc = crc8_dvb_s2(m_crc, c);
+
+ // If we've received the correct amount of bytes for payload
+ if (m_offset == m_packet.payloadSize) {
+ // Then we're up to the CRC
+ m_inputState = MSP_CHECKSUM_V2_NATIVE;
+ }
+ break;
+
+ case MSP_CHECKSUM_V2_NATIVE:
+ // Assert that the checksums match
+ if (m_crc == c) {
+ m_inputState = MSP_COMMAND_RECEIVED;
+ }
+ else {
+ DEBUG("CRC failure on MSP packet - Got %d expected %d", c, m_crc);
+ m_inputState = MSP_IDLE;
+ }
+ break;
+
+ default:
+ m_inputState = MSP_IDLE;
+ break;
+ }
+
+ // If we've successfully parsed a complete packet
+ // return true so the calling function knows that
+ // a new packet is ready.
+ if (m_inputState == MSP_COMMAND_RECEIVED) {
+ return true;
+ }
+ return false;
+}
+
+mspPacket_t*
+MSP::getReceivedPacket()
+{
+ return &m_packet;
+}
+
+void
+MSP::markPacketReceived()
+{
+ // Set input state to idle, ready to receive the next packet
+ // The current packet data will be discarded internally
+ m_inputState = MSP_IDLE;
+}
+
+bool
+MSP::sendPacket(mspPacket_t* packet, Stream* port)
+{
+ // Sanity check the packet before sending
+ if (packet->type != MSP_PACKET_COMMAND && packet->type != MSP_PACKET_RESPONSE) {
+ // Unsupported packet type (note: ignoring '!' until we know what it is)
+ return false;
+ }
+
+ if (packet->type == MSP_PACKET_RESPONSE && packet->payloadSize == 0) {
+ // Response packet with no payload
+ return false;
+ }
+
+ // Write out the framing chars
+ port->write('$');
+ port->write('X');
+
+ // Write out the packet type
+ if (packet->type == MSP_PACKET_COMMAND) {
+ port->write('<');
+ }
+ else if (packet->type == MSP_PACKET_RESPONSE) {
+ port->write('>');
+ }
+
+ // Subsequent bytes are contained in the crc
+ uint8_t crc = 0;
+
+ // Pack header struct into buffer
+ uint8_t headerBuffer[5];
+ mspHeaderV2_t* header = (mspHeaderV2_t*)&headerBuffer[0];
+ header->flags = packet->flags;
+ header->function = packet->function;
+ header->payloadSize = packet->payloadSize;
+
+ // Write out the header buffer, adding each byte to the crc
+ for (uint8_t i = 0; i < sizeof(mspHeaderV2_t); ++i) {
+ port->write(headerBuffer[i]);
+ crc = crc8_dvb_s2(crc, headerBuffer[i]);
+ }
+
+ // Write out the payload, adding each byte to the crc
+ for (uint16_t i = 0; i < packet->payloadSize; ++i) {
+ port->write(packet->payload[i]);
+ crc = crc8_dvb_s2(crc, packet->payload[i]);
+ }
+
+ // Write out the crc
+ port->write(crc);
+
+ return true;
+}
+uint8_t
+MSP::convertToByteArray(mspPacket_t* packet, uint8_t* byteArray)
+{
+ uint8_t bufferPos = 0;
+ // Sanity check the packet before converting
+ if (packet->type != MSP_PACKET_COMMAND && packet->type != MSP_PACKET_RESPONSE) {
+ // Unsupported packet type (note: ignoring '!' until we know what it is)
+ return 0;
+ }
+
+ if (packet->type == MSP_PACKET_RESPONSE && packet->payloadSize == 0) {
+ // Response packet with no payload
+ return 0;
+ }
+
+ // Write out the framing chars
+ byteArray[bufferPos++] = '$';
+ byteArray[bufferPos++] = 'X';
+
+ // Write out the packet type
+ if (packet->type == MSP_PACKET_COMMAND) {
+ byteArray[bufferPos++] = '<';
+ }
+ else if (packet->type == MSP_PACKET_RESPONSE) {
+ byteArray[bufferPos++] = '>';
+ }
+
+ // Subsequent bytes are contained in the crc
+ uint8_t crc = 0;
+
+ // Pack header struct into buffer
+ uint8_t headerBuffer[5];
+ mspHeaderV2_t* header = (mspHeaderV2_t*)&headerBuffer[0];
+ header->flags = packet->flags;
+ header->function = packet->function;
+ header->payloadSize = packet->payloadSize;
+
+ // Write out the header buffer, adding each byte to the crc
+ for (uint8_t i = 0; i < sizeof(mspHeaderV2_t); ++i) {
+ byteArray[bufferPos++] = headerBuffer[i];
+ crc = crc8_dvb_s2(crc, headerBuffer[i]);
+ }
+
+ // Write out the payload, adding each byte to the crc
+ for (uint16_t i = 0; i < packet->payloadSize; ++i) {
+ byteArray[bufferPos++] = packet->payload[i];
+ crc = crc8_dvb_s2(crc, packet->payload[i]);
+ }
+
+ // Write out the crc
+ byteArray[bufferPos++] = crc;
+
+ return bufferPos;
+}
+
+uint8_t
+MSP::getTotalPacketSize(mspPacket_t* packet)
+{
+ uint8_t totalSize = 0;
+
+ // framing chars
+ totalSize += sizeof('$');
+ totalSize += sizeof('X');
+
+ // packet type
+ if (packet->type == MSP_PACKET_COMMAND) {
+ totalSize += sizeof('<');
+ }
+ else if (packet->type == MSP_PACKET_RESPONSE) {
+ totalSize += sizeof('>');
+ }
+
+ // header
+ totalSize += sizeof(mspHeaderV2_t);
+
+ // payload
+ totalSize += packet->payloadSize;
+
+ // crc
+ totalSize++;
+
+ return totalSize;
+}
+
+bool
+MSP::awaitPacket(mspPacket_t* packet, Stream* port, uint32_t timeoutMillis)
+{
+ uint32_t requestTime = millis();
+
+ sendPacket(packet, port);
+
+ // wait up to milliseconds for a response, then bail out
+ while(millis() - requestTime < timeoutMillis)
+ {
+ while (port->available())
+ {
+ uint8_t data = port->read();
+ if (processReceivedByte(data))
+ {
+ return true;
+ }
+ }
+ }
+ DEBUG("MSP::awaitPacket Exceeded timeout while waiting for packet");
+ return false;
+}
\ No newline at end of file
diff --git a/lib/MSP/msp.h b/lib/MSP/msp.h
new file mode 100644
index 0000000..b8cc178
--- /dev/null
+++ b/lib/MSP/msp.h
@@ -0,0 +1,168 @@
+#pragma once
+
+#include
+#include "msptypes.h"
+
+// TODO: MSP_PORT_INBUF_SIZE should be changed to
+// dynamically allocate array length based on the payload size
+// Hardcoding payload size to 64 bytes for now, to allow enough space
+// for custom OSD text.
+#define MSP_PORT_INBUF_SIZE 64
+
+#define CHECK_PACKET_PARSING() \
+ if (packet->readError) {\
+ return;\
+ }
+
+typedef enum {
+ MSP_IDLE,
+ MSP_HEADER_START,
+ MSP_HEADER_X,
+
+ MSP_HEADER_V2_NATIVE,
+ MSP_PAYLOAD_V2_NATIVE,
+ MSP_CHECKSUM_V2_NATIVE,
+
+ MSP_COMMAND_RECEIVED
+} mspState_e;
+
+typedef enum {
+ MSP_PACKET_UNKNOWN,
+ MSP_PACKET_COMMAND,
+ MSP_PACKET_RESPONSE
+} mspPacketType_e;
+
+typedef struct __attribute__((packed)) {
+ uint8_t flags;
+ uint16_t function;
+ uint16_t payloadSize;
+} mspHeaderV2_t;
+
+typedef struct {
+ mspPacketType_e type;
+ uint8_t flags;
+ uint16_t function;
+ uint16_t payloadSize;
+ uint8_t payload[MSP_PORT_INBUF_SIZE];
+ uint16_t payloadReadIterator;
+ bool readError;
+ // Function that replace function enum with string
+ const char *get_func_name()
+ {
+ switch (function)
+ {
+ case MSP_ELRS_FUNC:
+ return "MSP_ELRS_FUNC";
+ case MSP_SET_RX_CONFIG:
+ return "MSP_SET_RX_CONFIG";
+ case MSP_VTX_CONFIG:
+ return "MSP_VTX_CONFIG";
+ case MSP_SET_VTX_CONFIG:
+ return "MSP_SET_VTX_CONFIG";
+ case MSP_EEPROM_WRITE:
+ return "MSP_EEPROM_WRITE";
+ case MSP_ELRS_RF_MODE:
+ return "MSP_ELRS_RF_MODE";
+ case MSP_ELRS_TX_PWR:
+ return "MSP_ELRS_TX_PWR";
+ case MSP_ELRS_TLM_RATE:
+ return "MSP_ELRS_TLM_RATE";
+ case MSP_ELRS_BIND:
+ return "MSP_ELRS_BIND";
+ case MSP_ELRS_MODEL_ID:
+ return "MSP_ELRS_MODEL_ID";
+ case MSP_ELRS_REQU_VTX_PKT:
+ return "MSP_ELRS_REQU_VTX_PKT";
+ case MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE:
+ return "MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE";
+ case MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE:
+ return "MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE";
+ case MSP_ELRS_SET_RX_WIFI_MODE:
+ return "MSP_ELRS_SET_RX_WIFI_MODE";
+ case MSP_ELRS_SET_RX_LOAN_MODE:
+ return "MSP_ELRS_SET_RX_LOAN_MODE";
+ case MSP_ELRS_GET_BACKPACK_VERSION:
+ return "MSP_ELRS_GET_BACKPACK_VERSION";
+ case MSP_ELRS_BACKPACK_CRSF_TLM:
+ return "MSP_ELRS_BACKPACK_CRSF_TLM";
+ case MSP_ELRS_SET_SEND_UID:
+ return "MSP_ELRS_SET_SEND_UID";
+ case MSP_ELRS_SET_OSD:
+ return "MSP_ELRS_SET_OSD";
+ case MSP_ELRS_BACKPACK_CONFIG:
+ return "MSP_ELRS_BACKPACK_CONFIG";
+ case MSP_ELRS_BACKPACK_CONFIG_TLM_MODE:
+ return "MSP_ELRS_BACKPACK_CONFIG_TLM_MODE";
+ case MSP_ELRS_BACKPACK_GET_CHANNEL_INDEX:
+ return "MSP_ELRS_BACKPACK_GET_CHANNEL_INDEX";
+ case MSP_ELRS_BACKPACK_SET_CHANNEL_INDEX:
+ return "MSP_ELRS_BACKPACK_SET_CHANNEL_INDEX";
+ case MSP_ELRS_BACKPACK_GET_FREQUENCY:
+ return "MSP_ELRS_BACKPACK_GET_FREQUENCY";
+ case MSP_ELRS_BACKPACK_GET_RECORDING_STATE:
+ return "MSP_ELRS_BACKPACK_GET_RECORDING_STATE";
+ default:
+ static char buffer[32];
+ snprintf(buffer, sizeof(buffer), "Unknown function: %u", function);
+ return buffer;
+ }
+ }
+
+ void reset()
+ {
+ type = MSP_PACKET_UNKNOWN;
+ flags = 0;
+ function = 0;
+ payloadSize = 0;
+ payloadReadIterator = 0;
+ readError = false;
+ }
+
+ void addByte(uint8_t b)
+ {
+ payload[payloadSize++] = b;
+ }
+
+ void makeResponse()
+ {
+ type = MSP_PACKET_RESPONSE;
+ }
+
+ void makeCommand()
+ {
+ type = MSP_PACKET_COMMAND;
+ }
+
+ uint8_t readByte()
+ {
+ if (payloadReadIterator >= payloadSize) {
+ // We are trying to read beyond the length of the payload
+ readError = true;
+ return 0;
+ }
+
+ return payload[payloadReadIterator++];
+ }
+} mspPacket_t;
+
+/////////////////////////////////////////////////
+
+class MSP
+{
+public:
+ MSP();
+ bool processReceivedByte(uint8_t c);
+ mspPacket_t* getReceivedPacket();
+ void markPacketReceived();
+ bool sendPacket(mspPacket_t* packet, Stream* port);
+ uint8_t convertToByteArray(mspPacket_t* packet, uint8_t* byteArray);
+ uint8_t getTotalPacketSize(mspPacket_t* packet);
+ bool awaitPacket(mspPacket_t* packet, Stream* port, uint32_t timeoutMillis);
+
+private:
+ mspState_e m_inputState;
+ uint16_t m_offset;
+ uint8_t m_inputBuffer[MSP_PORT_INBUF_SIZE];
+ mspPacket_t m_packet;
+ uint8_t m_crc;
+};
\ No newline at end of file
diff --git a/lib/MSP/msptypes.h b/lib/MSP/msptypes.h
new file mode 100644
index 0000000..0cda911
--- /dev/null
+++ b/lib/MSP/msptypes.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#define MSP_ELRS_FUNC 0x4578 // ['E','x']
+
+#define MSP_SET_RX_CONFIG 45
+#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight
+#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight
+#define MSP_EEPROM_WRITE 250 //in message no param
+
+// ELRS specific opcodes
+#define MSP_ELRS_RF_MODE 0x06
+#define MSP_ELRS_TX_PWR 0x07
+#define MSP_ELRS_TLM_RATE 0x08
+#define MSP_ELRS_BIND 0x09
+#define MSP_ELRS_MODEL_ID 0x0A
+#define MSP_ELRS_REQU_VTX_PKT 0x0B
+#define MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE 0x0C
+#define MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE 0x0D
+#define MSP_ELRS_SET_RX_WIFI_MODE 0x0E
+#define MSP_ELRS_SET_RX_LOAN_MODE 0x0F
+#define MSP_ELRS_GET_BACKPACK_VERSION 0x10
+#define MSP_ELRS_BACKPACK_CRSF_TLM 0x11
+#define MSP_ELRS_SET_SEND_UID 0x00B5
+#define MSP_ELRS_SET_OSD 0x00B6
+
+// Config opcodes
+#define MSP_ELRS_BACKPACK_CONFIG 0x30
+#define MSP_ELRS_BACKPACK_CONFIG_TLM_MODE 0x31
+
+// CRSF encapsulated msp defines
+#define ENCAPSULATED_MSP_PAYLOAD_SIZE 4
+#define ENCAPSULATED_MSP_FRAME_LEN 8
+
+// ELRS backpack protocol opcodes
+// See: https://docs.google.com/document/d/1u3c7OTiO4sFL2snI-hIo-uRSLfgBK4h16UrbA08Pd6U/edit#heading=h.1xw7en7jmvsj
+
+// outgoing, packets originating from the backpack or forwarded from the TX backpack to the VRx
+#define MSP_ELRS_BACKPACK_GET_CHANNEL_INDEX 0x0300
+#define MSP_ELRS_BACKPACK_SET_CHANNEL_INDEX 0x0301
+#define MSP_ELRS_BACKPACK_GET_FREQUENCY 0x0302
+#define MSP_ELRS_BACKPACK_SET_FREQUENCY 0x0303
+#define MSP_ELRS_BACKPACK_GET_RECORDING_STATE 0x0304
+#define MSP_ELRS_BACKPACK_SET_RECORDING_STATE 0x0305
+#define MSP_ELRS_BACKPACK_GET_VRX_MODE 0x0306
+#define MSP_ELRS_BACKPACK_SET_VRX_MODE 0x0307
+#define MSP_ELRS_BACKPACK_GET_RSSI 0x0308
+#define MSP_ELRS_BACKPACK_GET_BATTERY_VOLTAGE 0x0309
+#define MSP_ELRS_BACKPACK_GET_FIRMWARE 0x030A
+#define MSP_ELRS_BACKPACK_SET_BUZZER 0x030B
+#define MSP_ELRS_BACKPACK_SET_OSD_ELEMENT 0x030C
+#define MSP_ELRS_BACKPACK_SET_HEAD_TRACKING 0x030D // enable/disable head-tracking forwarding packets to the TX
+#define MSP_ELRS_BACKPACK_SET_RTC 0x030E
+
+// incoming, packets originating from the VRx
+#define MSP_ELRS_BACKPACK_SET_MODE 0x0380 // enable wifi/binding mode
+#define MSP_ELRS_BACKPACK_GET_VERSION 0x0381 // get the bacpack firmware version
+#define MSP_ELRS_BACKPACK_GET_STATUS 0x0382 // get the status of the backpack
+#define MSP_ELRS_BACKPACK_SET_PTR 0x0383 // forwarded back to TX backpack
\ No newline at end of file
diff --git a/lib/WEBSERVER/webserver.cpp b/lib/WEBSERVER/webserver.cpp
index b042ff2..eb2b184 100644
--- a/lib/WEBSERVER/webserver.cpp
+++ b/lib/WEBSERVER/webserver.cpp
@@ -5,7 +5,9 @@
#include
#include
#include
-
+#include
+#include "msp.h"
+#include "msptypes.h"
#include "debug.h"
static const uint8_t DNS_PORT = 53;
@@ -20,6 +22,129 @@ static const char *wifi_ap_ssid_prefix = "PhobosLT";
static const char *wifi_ap_password = "phoboslt";
static const char *wifi_ap_address = "20.0.0.1";
String wifi_ap_ssid;
+esp_now_peer_info_t peerInfo;
+uint8_t targetAddress[6];
+uint8_t uniAddress[6];
+MSP msp;
+
+int sendMSPViaEspnow(mspPacket_t *packet)
+{
+ int esp_err = -1;
+ uint8_t packetSize = msp.getTotalPacketSize(packet);
+ uint8_t nowDataOutput[packetSize];
+
+ uint8_t result = msp.convertToByteArray(packet, nowDataOutput);
+ DEBUG("Sending MSP data: ");
+ for (uint16_t i = 0; i < packetSize; ++i) {
+ DEBUG("%u ", nowDataOutput[i]);
+ }
+ DEBUG("\n");
+
+ if (!result)
+ {
+ return esp_err;
+ }
+
+ esp_err = esp_now_send(targetAddress, (uint8_t *) &nowDataOutput, packetSize);
+ if (esp_err != ESP_OK) {
+ DEBUG("Error sending ESP-NOW data: %s\n", esp_err_to_name(esp_err));
+ } else {
+ DEBUG("ESP-NOW data sent successfully\n");
+ }
+
+ return esp_err;
+}
+
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
+{
+ if (status == ESP_NOW_SEND_SUCCESS)
+ DEBUG("SENT OK\n");
+ else
+ DEBUG("SENT FAIL\n");
+}
+
+#define MSP_ELRS_SET_OSD_BEAT 0x01
+#define MSP_ELRS_SET_OSD_CLEAR 0x02
+#define MSP_ELRS_SET_OSD_WRITE 0x03
+#define MSP_ELRS_SET_OSD_SHOW 0x04
+
+mspPacket_t packet;
+void sendMSPOSDMessage(byte subfunction, char msg[]){
+
+ packet.reset();
+ packet.makeCommand();
+ packet.function = MSP_ELRS_SET_OSD;
+
+ packet.addByte(subfunction);
+ packet.addByte(3);
+ packet.addByte(3);
+ packet.addByte(0);
+ packet.addByte(0);
+ if (subfunction == MSP_ELRS_SET_OSD_WRITE)
+ {
+ for (int i = 0; i < strlen(msg); i++)
+ {
+ packet.addByte(msg[i]);
+ }
+ }
+ packet.addByte(0);
+
+ sendMSPViaEspnow(&packet);
+ usleep(100);
+}
+
+void sendElrsHello(){
+ char buf[] = "PLT TIMER CONNECTED!";
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_CLEAR, buf);
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_WRITE, buf);
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_SHOW, buf);
+}
+
+void getLapString(uint32_t lapTime, char *output) {
+ uint32_t min = lapTime / 1000 / 60;
+ uint32_t seconds = lapTime / 1000;
+ uint32_t milis = lapTime % 1000;
+
+ sprintf(output, "%02u:%02u.%03u", min, seconds, milis);
+}
+
+void sendElrsEvent(uint32_t lapTime) {
+ char lapTimeMsg[10];
+ getLapString(lapTime, lapTimeMsg);
+
+ char buf[20];
+ snprintf(buf, sizeof(buf), "TIME: %s", lapTimeMsg);
+ DEBUG("Sending ELRS OSD message: %s\n", buf);
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_CLEAR, buf);
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_WRITE, buf);
+ sendMSPOSDMessage(MSP_ELRS_SET_OSD_SHOW, buf);
+}
+
+void getBindingUID(char *phrase, uint8_t *bindingUID)
+{
+ String bindingPhraseFull = String("-DMY_BINDING_PHRASE=\"") + phrase + "\"";
+ int len = bindingPhraseFull.length();
+ char bindingPhrase[len + 1];
+ bindingPhraseFull.toCharArray(bindingPhrase, len + 1);
+
+ unsigned char md5Hash[16];
+ MD5Builder md5;
+ md5.begin();
+ md5.add(bindingPhrase);
+ md5.calculate();
+ md5.getBytes(md5Hash);
+
+ uint8_t uidBytes[6];
+ memcpy(uidBytes, md5Hash, 6);
+ DEBUG("\nBinding phrase uid: ");
+ for (int i = 0; i < 6; i++)
+ {
+ bindingUID[i] = uidBytes[i];
+ DEBUG("%u,", uidBytes[i]);
+ }
+ DEBUG(". Hello :) \n");
+}
+
void Webserver::init(Config *config, LapTimer *lapTimer, BatteryMonitor *batMonitor, Buzzer *buzzer, Led *l) {
@@ -56,6 +181,7 @@ void Webserver::sendRssiEvent(uint8_t rssi) {
events.send(buf, "rssi");
}
+
void Webserver::sendLaptimeEvent(uint32_t lapTime) {
if (!servicesStarted) return;
char buf[16];
@@ -66,6 +192,7 @@ void Webserver::sendLaptimeEvent(uint32_t lapTime) {
void Webserver::handleWebUpdate(uint32_t currentTimeMs) {
if (timer->isLapAvailable()) {
sendLaptimeEvent(timer->getLapTime());
+ sendElrsEvent(timer->getLapTime());
}
if (sendRssi && ((currentTimeMs - rssiSentMs) > WEB_RSSI_SEND_TIMEOUT_MS)) {
@@ -115,12 +242,45 @@ void Webserver::handleWebUpdate(uint32_t currentTimeMs) {
DEBUG("Changing to WiFi AP mode\n");
WiFi.disconnect();
- wifiMode = WIFI_AP;
- WiFi.setHostname(wifi_hostname); // hostname must be set before the mode is set to STA
- WiFi.mode(wifiMode);
- changeTimeMs = currentTimeMs;
+ WiFi.setTxPower(WIFI_POWER_19_5dBm);
+ WiFi.setHostname(wifi_hostname);
WiFi.softAPConfig(ipAddress, ipAddress, netMsk);
+ esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);
+ esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR);
WiFi.softAP(wifi_ap_ssid.c_str(), wifi_ap_password);
+ WiFi.mode(WIFI_AP_STA);
+ if (conf->getElrsBindPhrase()[0] != 0)
+ {
+
+ DEBUG("Enablind ESP NOW for backpack\n");
+ uint8_t bindingUID[6];
+ getBindingUID(conf->getElrsBindPhrase(), bindingUID);
+
+ memcpy(targetAddress, bindingUID, sizeof(targetAddress));
+ memcpy(uniAddress, bindingUID, sizeof(uniAddress));
+ uniAddress[0] = uniAddress[0] & ~0x01;
+
+ if (esp_now_init() != 0)
+ {
+ DEBUG("Error initializing ESP-NOW\n");
+ return;
+ }
+ esp_wifi_set_mac(WIFI_IF_STA, uniAddress);
+ memset(&peerInfo, 0, sizeof(peerInfo));
+ memcpy(peerInfo.peer_addr, targetAddress, 6);
+ peerInfo.channel = 1;
+ peerInfo.encrypt = false;
+ if (esp_now_add_peer(&peerInfo) != ESP_OK)
+ {
+ DEBUG("ESP-NOW failed to add peer\n");
+ return;
+ }
+
+ esp_now_register_send_cb(OnDataSent);
+ sendElrsHello();
+ }
+
+ DEBUG("Start services\n");
startServices();
buz->beep(1000);
led->on(1000);
diff --git a/src/main.cpp b/src/main.cpp
index 2190eec..ada129a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -32,6 +32,7 @@ static void initParallelTask() {
xTaskCreatePinnedToCore(parallelTask, "parallelTask", 3000, NULL, 0, &xTimerTask, 0);
}
+
void setup() {
DEBUG_INIT;
config.init();
diff --git a/targets/PhobosLT.ini b/targets/PhobosLT.ini
index e5978a2..1557e0a 100644
--- a/targets/PhobosLT.ini
+++ b/targets/PhobosLT.ini
@@ -16,3 +16,4 @@ lib_deps =
build_flags =
-DCONFIG_ASYNC_TCP_EVENT_QUEUE_SIZE=256
-DELEGANTOTA_USE_ASYNC_WEBSERVER=1
+ -DDEBUG_OUT=1