From dee9b7c89dd209496544684c19c319d33b2e071f Mon Sep 17 00:00:00 2001 From: cnheider Date: Wed, 12 Mar 2025 22:25:42 +0100 Subject: [PATCH 1/2] Rapidfire UserText OSD is now supported --- lib/rapidfire_SPI/rapidFIRE_SPI.cpp | 225 ++++++++++++++++ lib/rapidfire_SPI/rapidFIRE_SPI.h | 104 ++++++++ lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h | 60 +++++ src/rapidfire.cpp | 287 +++++++-------------- src/rapidfire.h | 43 +-- 5 files changed, 504 insertions(+), 215 deletions(-) create mode 100644 lib/rapidfire_SPI/rapidFIRE_SPI.cpp create mode 100644 lib/rapidfire_SPI/rapidFIRE_SPI.h create mode 100644 lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI.cpp b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp new file mode 100644 index 00000000..7841bb01 --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp @@ -0,0 +1,225 @@ +// rapidFIRE_SPI.cpp +// Copyright 2023 GOROman. + +#include "rapidFIRE_SPI.h" + +rapidFIRE_SPI::rapidFIRE_SPI(int pin_SCK, int pin_DATA, int pin_SS, int freq) { + assert(SPI_freq < RAPIDFIRE_SPI_MAX_CLOCK); + + SPI_pin_SCK = pin_SCK; + SPI_pin_DATA = pin_DATA; + SPI_pin_SS = pin_SS; + SPI_freq = freq; + + spi = new SPIClass; +} + +rapidFIRE_SPI::~rapidFIRE_SPI() { delete spi; } + +RF_RESULT rapidFIRE_SPI::begin() { + // Init SPI + pinMode(SPI_pin_SCK, OUTPUT); + pinMode(SPI_pin_DATA, OUTPUT); + pinMode(SPI_pin_SS, OUTPUT); + + // To enable SPI mode, set CS1, CS2, CS3 high, and then within 100-400ms set + // them all low. + digitalWrite(SPI_pin_SCK, HIGH); + digitalWrite(SPI_pin_DATA, HIGH); + digitalWrite(SPI_pin_SS, HIGH); + + delay(RAPIDFIRE_SPI_MODE_ENABLE_DELAY); + + digitalWrite(SPI_pin_SCK, LOW); + digitalWrite(SPI_pin_DATA, LOW); + digitalWrite(SPI_pin_SS, LOW); + + delay(10); + + digitalWrite(SPI_pin_SS, HIGH); + + return RF_RESULT_OK; +} + +RF_RESULT rapidFIRE_SPI::end() { return RF_RESULT_OK; } + +RF_RESULT rapidFIRE_SPI::recvCommand(uint8_t *data, size_t len) { + assert(data != NULL); + assert(len != 0); + + if (data == NULL) { + return RF_RESULT_ERROR; + } + + byte recv_len = 0x00; + byte recv_sum = 0x00; + +#if defined(PLATFORM_ESP32) + spi->begin(SPI_pin_SCK, SPI_pin_DATA, -1, SPI_pin_SS); +#else + spi->begin(); + spi->pins(SPI_pin_SCK, SPI_pin_DATA, -1, SPI_pin_SS); +#endif + + spi->setFrequency(SPI_freq); + spi->setDataMode(SPI_MODE0); + spi->setBitOrder(MSBFIRST); + spi->setHwCs(true); + + recv_len = spi->transfer(0); + recv_sum = spi->transfer(0); + for (int i = 0; i < len; i++) { + data[i] = spi->transfer(0); + } + + spi->end(); + spi->setHwCs(false); + + byte csum = recv_len; + for (int i = 0; i < len; ++i) { + // Serial.printf("Read[%d]:0x%02x(%3d) '\%c'\n", i, data[i], data[i], + // data[i]); + csum += data[i]; + } + + Serial.printf("CSUM:recv:%02x==calc:%02x LEN:recv:%d==%d %s\n", recv_sum, + csum, recv_len, len, recv_sum == csum ? "" : "NG!"); + + if (recv_sum != csum) { + // return RF_RESULT_ERROR_CHECKSUM_MISSMATCH; // Checksum missmatch. + } + return RF_RESULT_OK; +} + +RF_RESULT rapidFIRE_SPI::sendCommand(uint16_t command, byte *data, size_t len) { + uint8_t csum = (command >> 8) + (command & 0xff) + len; + + for (int i = 0; i < len; ++i) { + csum += data[i]; + } + +#if defined(PLATFORM_ESP32) + spi->begin(SPI_pin_SCK, SPI_pin_DATA, -1, SPI_pin_SS); +#else + spi->begin(); + spi->pins(SPI_pin_SCK, SPI_pin_DATA, -1, SPI_pin_SS); +#endif + spi->setFrequency(SPI_freq); + spi->setDataMode(SPI_MODE0); + spi->setBitOrder(MSBFIRST); + spi->setHwCs(true); + + spi->write16(command); + spi->write(len); + spi->write(csum); + spi->writeBytes(data, len); + spi->end(); + + spi->setHwCs(false); + +#if 0 + Serial.printf("sendCommand: '%c'(%02x) '%c'(%02x) LEN:%d SUM:%02x DATA:[", command>>8, command>>8, command&0xff, command&0xff, len, csum); + for (int i = 0; i < len; ++i) + { + Serial.printf(" %02x", data[i]); + } + Serial.printf("]\n"); +#endif + return RF_RESULT_OK; +} + +RF_RESULT rapidFIRE_SPI::getFirmwareVersion(QUERY_FIRMWARE_VERSION *version) { + assert(version); + if (version == NULL) { + return RF_RESULT_ERROR; + } + + RF_RESULT res = sendCommand(RAPIDFIRE_CMD_FIRMWARE_VERSION); + if (res != RF_RESULT_OK) { + return res; + } + byte data[6] = {0}; + res = recvCommand(data, sizeof(data)); + if (res == RF_RESULT_OK) { + for (int i = 0; i < 3; ++i) { + version->oled[i] = data[0 + i]; + version->core[i] = data[3 + i]; + } + } + + return res; +} + +RF_RESULT rapidFIRE_SPI::getRSSI(QUERY_RSSI *rssi) { + assert(rssi); + if (rssi == NULL) { + return RF_RESULT_ERROR; + } + RF_RESULT res = sendCommand(RAPIDFIRE_CMD_RSSI); + if (res != RF_RESULT_OK) { + return res; + } + byte data[8] = {0}; + res = recvCommand(data, sizeof(data)); + if (res == RF_RESULT_OK) { + rssi->raw_rx1 = data[0] | data[1] << 8; + rssi->raw_rx2 = data[2] | data[3] << 8; + rssi->scaled_rx1 = data[4] | data[5] << 8; + rssi->scaled_rx2 = data[6] | data[7] << 8; + } + return res; +} + +RF_RESULT rapidFIRE_SPI::getVoltage(QUERY_VOLTAGE *voltage) { + assert(voltage); + if (voltage == NULL) { + return RF_RESULT_ERROR; + } + + RF_RESULT res = sendCommand(RAPIDFIRE_CMD_VOLTAGE); + if (res != RF_RESULT_OK) { + return res; + } + byte data[2] = {0}; + res = recvCommand(data, sizeof(data)); + if (res == RF_RESULT_OK) { + int temp = data[0] | data[1] << 8; + voltage->voltage = (float)temp / 1000.0f; + } + + return res; +} + +RF_RESULT rapidFIRE_SPI::buzzer() { + return sendCommand(RAPIDFIRE_CMD_SOUND_BUZZER); +} + +RF_RESULT rapidFIRE_SPI::setOSDUserText(char *text) { + return sendCommand(RAPIDFIRE_CMD_SET_OSD_USER_TEXT, (byte *)text, + strlen(text)); +} + +RF_RESULT rapidFIRE_SPI::setOSDMode(OSDMODE mode) { + byte data[] = {(byte)mode}; + return sendCommand(RAPIDFIRE_CMD_SET_OSD_MODE, data, sizeof(data)); +} + +RF_RESULT rapidFIRE_SPI::setRXModule(RXMODULE module) { + byte data[] = {(byte)module}; + return sendCommand(RAPIDFIRE_CMD_SET_RX_MODULE, data, sizeof(data)); +} + +RF_RESULT rapidFIRE_SPI::setChannel(byte channel) { + byte data[] = {channel}; + return sendCommand(RAPIDFIRE_CMD_SET_CHANNEL, data, sizeof(data)); +} + +RF_RESULT rapidFIRE_SPI::setBand(BAND band) { + byte data[] = {band}; + return sendCommand(RAPIDFIRE_CMD_SET_BAND, data, sizeof(data)); +} + +RF_RESULT rapidFIRE_SPI::setRapidfireMode(RAPIDFIREMODE mode) { + byte data[] = {(byte)mode}; + return sendCommand(RAPIDFIRE_CMD_SET_RAPIDFIRE_MODE, data, sizeof(data)); +} diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI.h b/lib/rapidfire_SPI/rapidFIRE_SPI.h new file mode 100644 index 00000000..d20c1121 --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.h @@ -0,0 +1,104 @@ +// rapidFIRE_SPI.h +// Copyright 2023 GOROman. + +#pragma once +#include "rapidFIRE_SPI_Protocol.h" +#include +#include + +struct QUERY_RSSI { + int16_t raw_rx1; + int16_t raw_rx2; + int16_t scaled_rx1; + int16_t scaled_rx2; +}; + +struct QUERY_FIRMWARE_VERSION { + byte oled[3]; // rapidFIRE OLED firmware version. + byte core[3]; // rapidFIRE core firmware version. +}; + +struct QUERY_VOLTAGE { + float voltage; // 4.222 V +}; + +typedef int RF_RESULT; + +enum { + RF_RESULT_OK = 0, + RF_RESULT_ERROR = -1, + RF_RESULT_ERROR_CHECKSUM_MISSMATCH = -2, +}; + +class SPIClass; + +class rapidFIRE_SPI { + int SPI_pin_SCK = -1; + int SPI_pin_DATA = -1; + int SPI_pin_SS = -1; + int SPI_freq; + + SPIClass *spi = NULL; + +public: + enum BAND { + BAND_F = 0x01, // - ImmersionRC / FatShark + BAND_R = 0x02, // - RaceBand + BAND_E = 0x03, // - Boscam E + BAND_B = 0x04, // - Boscam B + BAND_A = 0x05, // - Boscam A + BAND_L = 0x06, // - LowRace + BAND_X = 0x07, // - Band X + }; + + enum RXMODULE { + BOTH = 0x00, // - Both + UPPER = 0x01, // - Upper + LOWER = 0x02, // - Lower + }; + + enum OSDMODE { + OFF = 0, // - Off + LOCKONLY = 1, // - LockOnly + DEFAULTMODE = 2, // - Default + LOCKANDSTANDARD = 3, // - LockAndStandard + RSSIBARSLITE = 4, // - RSSIBarsLite + RSSIBARS = 5, // - RSSIBars + UNIQUIE_ID = 6, // - Unique ID + INTERNAL_USE = 7, // - Internal use + USERTEXT = 8, // - UserText + + }; + enum RAPIDFIREMODE { + RAPIDFIRE_1 = 0x00, // - RapidFIRE #1 + RAPIDFIRE_2 = 0x01, // - RapidFIRE #2 + LEGACY = 0x02, // - Legacy + }; + +private: + RF_RESULT recvCommand(uint8_t *data, size_t len); + RF_RESULT sendCommand(uint16_t command, byte *data = NULL, size_t len = 0); + +public: + rapidFIRE_SPI(int pin_SCK, int pin_DATA, int pin_SS, int freq = 60000); + ~rapidFIRE_SPI(); + + RF_RESULT begin(); + RF_RESULT end(); + + // Query + RF_RESULT getFirmwareVersion(QUERY_FIRMWARE_VERSION *version); + RF_RESULT getRSSI(QUERY_RSSI *rssi); + RF_RESULT getVoltage(QUERY_VOLTAGE *voltage); + + // Action + RF_RESULT buzzer(); + + // Command + RF_RESULT setOSDUserText(char *text); + RF_RESULT setOSDMode(OSDMODE mode); + RF_RESULT setRXModule(RXMODULE module); + RF_RESULT setChannel(byte channel); + RF_RESULT setBand(BAND band); + RF_RESULT setRapidfireMode(RAPIDFIREMODE mode); +}; diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h b/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h new file mode 100644 index 00000000..0fb7519c --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h @@ -0,0 +1,60 @@ +// ImmersionRC Rapidfire - SPI Mode Programming +// +// rapidFIRE_SPI_Protocol.h +// Copyright 2023 GOROman. + +#pragma once + +// The CS1, CS2, CS3 pins are used as the SPI interface. These are normally used as a 3-bit +// binary interface to communicate the goggle selected channel to the module. +// The SPI interface is configured so that the module is the slave (allowing several modules to +// be connected to the same bus), with CPOL = 0, CPHA = 0, MSB first. +// The speed should be limited to a clock rate of about 80kHz. + +#define RAPIDFIRE_SPI_CS1 1 +#define RAPIDFIRE_SPI_CS2 2 +#define RAPIDFIRE_SPI_CS3 3 + +#define RAPIDFIRE_SPI_CPOL 0 +#define RAPIDFIRE_SPI_CPHA 0 + +#define RAPIDFIRE_SPI_BIT (SPI_MSBFIRST) + +#define RAPIDFIRE_SPI_MAX_CLOCK (80000) // 80kHz + +#define RAPIDFIRE_SPI_MODE_ENABLE_DELAY 100 // ms + +// SPI Protocol +// ------------ +// +// Command Heade +// +// | cmd | dir | len | csum | data0 | ... | | +// +// Where Csum is computed as the 8 - bit checksum of all header bytes, Cmd, Dir, Len, and all(optional) data bytes. +// +// Query Header +// | len | csum | data0 | ... | | +// +// Where Csum is computed as the 8-bit checksum of Len, and all data bytes. + +// SPI Commands +// ------------ +#define RAPIDFIRE_CMD(Cmd, Dir) (Cmd << 8 | Dir) + +// Query +#define RAPIDFIRE_CMD_FIRMWARE_VERSION RAPIDFIRE_CMD('F', '?') // - Firmware Version, Query +#define RAPIDFIRE_CMD_VOLTAGE RAPIDFIRE_CMD('V', '?') // - Voltage, Query +#define RAPIDFIRE_CMD_RSSI RAPIDFIRE_CMD('R', '?') // - RSSI, Query + +// Action +#define RAPIDFIRE_CMD_SOUND_BUZZER RAPIDFIRE_CMD('S', '>') // - Buzzer + +// Command +#define RAPIDFIRE_CMD_SET_OSD_USER_TEXT RAPIDFIRE_CMD('T', '=') +#define RAPIDFIRE_CMD_SET_OSD_MODE RAPIDFIRE_CMD('O', '=') +#define RAPIDFIRE_CMD_SET_RX_MODULE RAPIDFIRE_CMD('M', '=') +#define RAPIDFIRE_CMD_SET_CHANNEL RAPIDFIRE_CMD('C', '=') +#define RAPIDFIRE_CMD_SET_BAND RAPIDFIRE_CMD('B', '=') +#define RAPIDFIRE_CMD_SET_RAPIDFIRE_MODE RAPIDFIRE_CMD('D', '=') + diff --git a/src/rapidfire.cpp b/src/rapidfire.cpp index a8ca76e0..fcca45fd 100644 --- a/src/rapidfire.cpp +++ b/src/rapidfire.cpp @@ -1,225 +1,120 @@ #include "rapidfire.h" -#include #include "logging.h" -void -Rapidfire::Init() -{ - ModuleBase::Init(); +static rapidFIRE_SPI rapidfire = rapidFIRE_SPI(PIN_CLK, PIN_MOSI, PIN_CS); - delay(VRX_BOOT_DELAY); - EnableSPIMode(); // https://github.com/ExpressLRS/ExpressLRS/pull/1489 & https://github.com/ExpressLRS/Backpack/pull/65 +void Rapidfire::Init() { + ModuleBase::Init(); - pinMode(PIN_MOSI, INPUT); - pinMode(PIN_CLK, INPUT); - pinMode(PIN_CS, INPUT); + delay(VRX_BOOT_DELAY); - DBGLN("Rapid Fire init"); -} + rapidfire.begin(); -void -Rapidfire::EnableSPIMode() -{ - // Set pins to output to configure rapidfire in SPI mode - pinMode(PIN_MOSI, OUTPUT); - pinMode(PIN_CLK, OUTPUT); - pinMode(PIN_CS, OUTPUT); - - // put the RF into SPI mode by pulling all 3 pins high, - // then low within 100-400ms - digitalWrite(PIN_MOSI, HIGH); - digitalWrite(PIN_CLK, HIGH); - digitalWrite(PIN_CS, HIGH); - delay(200); - digitalWrite(PIN_MOSI, LOW); - digitalWrite(PIN_CLK, LOW); - digitalWrite(PIN_CS, LOW); - delay(200); - - SPIModeEnabled = true; - - DBGLN("SPI config complete"); + DBGLN("Rapid Fire init"); } -void -Rapidfire::SendBuzzerCmd() -{ - DBGLN("Beep!"); - - uint8_t cmd[4]; - cmd[0] = RF_API_BEEP_CMD; // 'S' - cmd[1] = RF_API_DIR_GRTHAN; // '>' - cmd[2] = 0x00; // len - cmd[3] = crc8(cmd, 3); - - // rapidfire sometimes missed a pkt, so send 3x - SendSPI(cmd, 4); - SendSPI(cmd, 4); - SendSPI(cmd, 4); -} +void Rapidfire::SetOSD(mspPacket_t *packet) { + int len = packet->payloadSize; + if (len < 4) { + return; + } -void -Rapidfire::SendIndexCmd(uint8_t index) -{ - uint8_t newBand = index / 8 + 1; - uint8_t newChannel = index % 8; + for (int i = 0; i < 4; i++) { //payload = [0x03, row, col, 0] + packet->readByte(); // skip the first 4 bytes + } + len -= 4; // subtract the first 4 bytes - SendBandCmd(newBand); - delay(100); - SendChannelCmd(newChannel); -} + if (len > MAX_LENGTH_TEXT) { + len = MAX_LENGTH_TEXT; + } -void -Rapidfire::SendChannelCmd(uint8_t channel) -{ - // ELRS channel is zero based, need to add 1 - channel++; - - DBG("Setting new channel "); - DBGLN("%x", channel); - - uint8_t cmd[5]; - cmd[0] = RF_API_CHANNEL_CMD; // 'C' - cmd[1] = RF_API_DIR_EQUAL; // '=' - cmd[2] = 0x01; // len - cmd[3] = channel; // temporarily set byte 4 to channel for crc calc - cmd[3] = crc8(cmd, 4); // reset byte 4 to crc - cmd[4] = channel; // assign channel to correct byte 5 - - // rapidfire sometimes misses pkts, so send each one 3x - for (int i = 0; i < SPAM_COUNT; i++) - { - SendSPI(cmd, 5); - } -} + for (int i = 0; i < len; i++) { + m_textBuffer[i] = packet->readByte(); + } -void -Rapidfire::SendBandCmd(uint8_t band) -{ - DBG("Setting new band "); - DBGLN("%x", band); - - // ELRS bands - // 0x01 - Boscam A - // 0x02 - Boscam B - // 0x03 - Boscam E - // 0x04 - ImmersionRC/FatShark - // 0x05 - RaceBand - // 0x06 - LowRace - - // rapidfire bands - // 0x01 - ImmersionRC/FatShark - // 0x02 - RaceBand - // 0x03 - Boscam E - // 0x04 - Boscam B - // 0x05 - Boscam A - // 0x06 - LowRace - // 0x07 - Band X - - // convert ELRS band index to IMRC band index: - uint8_t imrcBand; - switch (band) - { - case 0x01: - imrcBand = 0x05; - break; - case 0x02: - imrcBand = 0x04; - break; - case 0x03: - imrcBand = 0x03; - break; - case 0x04: - imrcBand = 0x01; - break; - case 0x05: - imrcBand = 0x02; - break; - case 0x06: - imrcBand = 0x06; - break; - default: - imrcBand = 0x01; - break; - } + DisplayTextBuffer(); +} - uint8_t cmd[5]; - cmd[0] = RF_API_BAND_CMD; // 'C' - cmd[1] = RF_API_DIR_EQUAL; // '=' - cmd[2] = 0x01; // len - cmd[3] = imrcBand; // temporarily set byte 4 to band for crc calc - cmd[3] = crc8(cmd, 4); // reset byte 4 to crc - cmd[4] = imrcBand; // assign band to correct byte 5 - - // rapidfire sometimes misses pkts, so send each one 3x - for (int i = 0; i < SPAM_COUNT; i++) - { - SendSPI(cmd, 5); +void Rapidfire::DisplayTextBuffer() { + if (m_displayStartMillis == 0) { + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::USERTEXT); + delay(DELAY_BETWEEN_SPI_PKT); } -} + } -void -Rapidfire::SendSPI(uint8_t* buf, uint8_t bufLen) -{ - if (!SPIModeEnabled) EnableSPIMode(); + // only send the text once to the rapidfire to avoid failure + rapidfire.setOSDUserText(m_textBuffer); // send the text to the rapidfire - uint32_t periodMicroSec = 1000000 / BIT_BANG_FREQ; + memset(m_textBuffer, 0, MAX_LENGTH_TEXT); - pinMode(PIN_MOSI, OUTPUT); - pinMode(PIN_CLK, OUTPUT); - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_MOSI, LOW); - digitalWrite(PIN_CLK, LOW); - digitalWrite(PIN_CS, HIGH); + m_displayStartMillis = millis(); +} - delayMicroseconds(periodMicroSec); +void Rapidfire::SetRecordingState(uint8_t recordingState, uint16_t _delay){ + sprintf(m_textBuffer, "Is Recording %5d", recordingState); - digitalWrite(PIN_CS, LOW); - delay(100); + DisplayTextBuffer(); +} - // debug code for printing SPI pkt - for (int i = 0; i < bufLen; ++i) - { - uint8_t bufByte = buf[i]; +void Rapidfire::SendHeadTrackingEnableCmd(bool enable) { + sprintf(m_textBuffer, "Head Tracking %5d", enable); - DBG("%x", bufByte); - DBG(","); + DisplayTextBuffer(); +} - for (uint8_t k = 0; k < 8; k++) - { - // digitalWrite takes about 0.5us, so it is not taken into account with delays. - digitalWrite(PIN_CLK, LOW); - delayMicroseconds(periodMicroSec / 4); - digitalWrite(PIN_MOSI, bufByte & 0x80); - delayMicroseconds(periodMicroSec / 4); - digitalWrite(PIN_CLK, HIGH); - delayMicroseconds(periodMicroSec / 2); - - bufByte <<= 1; - } - } - DBGLN(""); +void Rapidfire::SendIndexCmd(uint8_t index) { + uint8_t newBand = index / 8 + 1; + uint8_t newChannel = index % 8; + + rapidFIRE_SPI::BAND imrcBand; // convert ELRS band index to IMRC band index: + switch (newBand) { + case 0x01: // Boscam A + imrcBand = rapidFIRE_SPI::BAND::BAND_A; + break; + case 0x02: // Boscam B + imrcBand = rapidFIRE_SPI::BAND::BAND_B; + break; + case 0x03: // Boscam E + imrcBand = rapidFIRE_SPI::BAND::BAND_E; + break; + case 0x04: // ImmersionRC/FatShark + imrcBand = rapidFIRE_SPI::BAND::BAND_F; + break; + case 0x05: // RaceBand + imrcBand = rapidFIRE_SPI::BAND::BAND_R; + break; + case 0x06: // LowRace + imrcBand = rapidFIRE_SPI::BAND::BAND_L; + break; + default: // ImmersionRC/FatShark + imrcBand = rapidFIRE_SPI::BAND::BAND_F; + break; + } - digitalWrite(PIN_MOSI, LOW); - digitalWrite(PIN_CLK, LOW); - digitalWrite(PIN_CS, HIGH); - delay(100); + newChannel++; // ELRS channel is zero based, need to add 1 - pinMode(PIN_MOSI, INPUT); - pinMode(PIN_CLK, INPUT); - pinMode(PIN_CS, INPUT); + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setBand(imrcBand); + delay(DELAY_BETWEEN_SPI_PKT); + rapidfire.setChannel(newChannel); + delay(DELAY_BETWEEN_SPI_PKT); + } } -// CRC function for IMRC rapidfire API -// Input: byte array, array length -// Output: crc byte -uint8_t -Rapidfire::crc8(uint8_t* buf, uint8_t bufLen) +void +Rapidfire::Loop(uint32_t now) { - uint32_t sum = 0; - for (uint8_t i = 0; i < bufLen; ++i) - { - sum += buf[i]; - } - return sum & 0xFF; + ModuleBase::Loop(now); + + if (m_displayStartMillis != 0) { + if (now - m_displayStartMillis >= TIMEOUT_SET_OSD) + { + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::DEFAULTMODE); + delay(DELAY_BETWEEN_SPI_PKT); + } + m_displayStartMillis = 0; + } + } } diff --git a/src/rapidfire.h b/src/rapidfire.h index b70b3ca8..e8b5f433 100644 --- a/src/rapidfire.h +++ b/src/rapidfire.h @@ -1,31 +1,36 @@ #pragma once #include "module_base.h" +#include "msp.h" +#include "msptypes.h" +#include "rapidFIRE_SPI.h" +#include "rapidFIRE_SPI_Protocol.h" #include +#include -#define VRX_BOOT_DELAY 2000 +#define VRX_BOOT_DELAY 2000 // 2 seconds delay before sending any packets to rapidfire -#define BIT_BANG_FREQ 1000 -#define SPAM_COUNT 3 +#define DELAY_BETWEEN_SPI_PKT 100 // 100ms delay between each SPI packet -#define RF_API_DIR_GRTHAN 0x3E // '>' -#define RF_API_DIR_EQUAL 0x3D // '=' -#define RF_API_BEEP_CMD 0x53 // 'S' -#define RF_API_CHANNEL_CMD 0x43 // 'C' -#define RF_API_BAND_CMD 0x42 // 'B' +#define SPAM_COUNT 3 // rapidfire sometimes missed a pkt, so send 3x -class Rapidfire : public ModuleBase -{ +#define MAX_LENGTH_TEXT 25 // max length of text to display on rapidfire osd + +#define TIMEOUT_SET_OSD 3000 // 3 seconds + +class Rapidfire : public ModuleBase { public: - void Init(); - void SendBuzzerCmd(); - void SendIndexCmd(uint8_t index); - void SendChannelCmd(uint8_t channel); - void SendBandCmd(uint8_t band); + void Init(); + void Loop(uint32_t now); + void SendIndexCmd(uint8_t index); + void SendChannelCmd(uint8_t channel); + void SendBandCmd(uint8_t band); + void SendHeadTrackingEnableCmd(bool enable); + void SetRecordingState(uint8_t recordingState, uint16_t delay); + void SetOSD(mspPacket_t *packet); private: - void SendSPI(uint8_t* buf, uint8_t bufLen); - void EnableSPIMode(); - uint8_t crc8(uint8_t* buf, uint8_t bufLen); - bool SPIModeEnabled = false; + uint32_t m_displayStartMillis = 0; + char m_textBuffer[MAX_LENGTH_TEXT]; + void DisplayTextBuffer(); }; From ec52c63205fa33446bc68da7a6c6e5ac36803596 Mon Sep 17 00:00:00 2001 From: Christian Heider Lindbjerg Date: Sun, 16 Mar 2025 00:10:18 +0100 Subject: [PATCH 2/2] Implemented a fixed size queue for delayed displaying of received packets --- lib/rapidfire_SPI/rapidFIRE_SPI.cpp | 22 +- lib/rapidfire_SPI/rapidFIRE_SPI.h | 5 +- lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h | 29 +-- src/rapidfire.cpp | 240 +++++++++++++++------ src/rapidfire.h | 30 ++- 5 files changed, 224 insertions(+), 102 deletions(-) diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI.cpp b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp index 7841bb01..fcba8192 100644 --- a/lib/rapidfire_SPI/rapidFIRE_SPI.cpp +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp @@ -4,7 +4,7 @@ #include "rapidFIRE_SPI.h" rapidFIRE_SPI::rapidFIRE_SPI(int pin_SCK, int pin_DATA, int pin_SS, int freq) { - assert(SPI_freq < RAPIDFIRE_SPI_MAX_CLOCK); + assert(freq < RAPIDFIRE_SPI_MAX_CLOCK); SPI_pin_SCK = pin_SCK; SPI_pin_DATA = pin_DATA; @@ -68,7 +68,7 @@ RF_RESULT rapidFIRE_SPI::recvCommand(uint8_t *data, size_t len) { recv_len = spi->transfer(0); recv_sum = spi->transfer(0); - for (int i = 0; i < len; i++) { + for (size_t i = 0; i < len; i++) { data[i] = spi->transfer(0); } @@ -76,15 +76,10 @@ RF_RESULT rapidFIRE_SPI::recvCommand(uint8_t *data, size_t len) { spi->setHwCs(false); byte csum = recv_len; - for (int i = 0; i < len; ++i) { - // Serial.printf("Read[%d]:0x%02x(%3d) '\%c'\n", i, data[i], data[i], - // data[i]); + for (size_t i = 0; i < len; ++i) { csum += data[i]; } - Serial.printf("CSUM:recv:%02x==calc:%02x LEN:recv:%d==%d %s\n", recv_sum, - csum, recv_len, len, recv_sum == csum ? "" : "NG!"); - if (recv_sum != csum) { // return RF_RESULT_ERROR_CHECKSUM_MISSMATCH; // Checksum missmatch. } @@ -94,7 +89,7 @@ RF_RESULT rapidFIRE_SPI::recvCommand(uint8_t *data, size_t len) { RF_RESULT rapidFIRE_SPI::sendCommand(uint16_t command, byte *data, size_t len) { uint8_t csum = (command >> 8) + (command & 0xff) + len; - for (int i = 0; i < len; ++i) { + for (size_t i = 0; i < len; ++i) { csum += data[i]; } @@ -104,6 +99,7 @@ RF_RESULT rapidFIRE_SPI::sendCommand(uint16_t command, byte *data, size_t len) { spi->begin(); spi->pins(SPI_pin_SCK, SPI_pin_DATA, -1, SPI_pin_SS); #endif + spi->setFrequency(SPI_freq); spi->setDataMode(SPI_MODE0); spi->setBitOrder(MSBFIRST); @@ -117,14 +113,6 @@ RF_RESULT rapidFIRE_SPI::sendCommand(uint16_t command, byte *data, size_t len) { spi->setHwCs(false); -#if 0 - Serial.printf("sendCommand: '%c'(%02x) '%c'(%02x) LEN:%d SUM:%02x DATA:[", command>>8, command>>8, command&0xff, command&0xff, len, csum); - for (int i = 0; i < len; ++i) - { - Serial.printf(" %02x", data[i]); - } - Serial.printf("]\n"); -#endif return RF_RESULT_OK; } diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI.h b/lib/rapidfire_SPI/rapidFIRE_SPI.h index d20c1121..f8e623ce 100644 --- a/lib/rapidfire_SPI/rapidFIRE_SPI.h +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.h @@ -36,7 +36,7 @@ class rapidFIRE_SPI { int SPI_pin_SCK = -1; int SPI_pin_DATA = -1; int SPI_pin_SS = -1; - int SPI_freq; + int SPI_freq = -1; SPIClass *spi = NULL; @@ -71,7 +71,8 @@ class rapidFIRE_SPI { }; enum RAPIDFIREMODE { RAPIDFIRE_1 = 0x00, // - RapidFIRE #1 - RAPIDFIRE_2 = 0x01, // - RapidFIRE #2 + RAPIDFIRE_2 = 0x01, // - RapidFIRE #2 # Seems to have compatibility issues + // with OSD UserText LEGACY = 0x02, // - Legacy }; diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h b/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h index 0fb7519c..eb33ab84 100644 --- a/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h +++ b/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h @@ -5,11 +5,12 @@ #pragma once -// The CS1, CS2, CS3 pins are used as the SPI interface. These are normally used as a 3-bit -// binary interface to communicate the goggle selected channel to the module. -// The SPI interface is configured so that the module is the slave (allowing several modules to -// be connected to the same bus), with CPOL = 0, CPHA = 0, MSB first. -// The speed should be limited to a clock rate of about 80kHz. +// The CS1, CS2, CS3 pins are used as the SPI interface. These are normally used +// as a 3-bit binary interface to communicate the goggle selected channel to the +// module. The SPI interface is configured so that the module is the slave +// (allowing several modules to be connected to the same bus), with CPOL = 0, +// CPHA = 0, MSB first. The speed should be limited to a clock rate of about +// 80kHz. #define RAPIDFIRE_SPI_CS1 1 #define RAPIDFIRE_SPI_CS2 2 @@ -18,12 +19,15 @@ #define RAPIDFIRE_SPI_CPOL 0 #define RAPIDFIRE_SPI_CPHA 0 -#define RAPIDFIRE_SPI_BIT (SPI_MSBFIRST) +#define RAPIDFIRE_SPI_BIT SPI_MSBFIRST -#define RAPIDFIRE_SPI_MAX_CLOCK (80000) // 80kHz +#define RAPIDFIRE_SPI_MAX_CLOCK 80000 // 80kHz #define RAPIDFIRE_SPI_MODE_ENABLE_DELAY 100 // ms +#define RAPIDFIRE_MAX_LENGTH_TEXT \ + 25 // max length of text to display on rapidfire osd + // SPI Protocol // ------------ // @@ -31,7 +35,8 @@ // // | cmd | dir | len | csum | data0 | ... | | // -// Where Csum is computed as the 8 - bit checksum of all header bytes, Cmd, Dir, Len, and all(optional) data bytes. +// Where Csum is computed as the 8 - bit checksum of all header bytes, Cmd, Dir, +// Len, and all(optional) data bytes. // // Query Header // | len | csum | data0 | ... | | @@ -43,9 +48,10 @@ #define RAPIDFIRE_CMD(Cmd, Dir) (Cmd << 8 | Dir) // Query -#define RAPIDFIRE_CMD_FIRMWARE_VERSION RAPIDFIRE_CMD('F', '?') // - Firmware Version, Query -#define RAPIDFIRE_CMD_VOLTAGE RAPIDFIRE_CMD('V', '?') // - Voltage, Query -#define RAPIDFIRE_CMD_RSSI RAPIDFIRE_CMD('R', '?') // - RSSI, Query +#define RAPIDFIRE_CMD_FIRMWARE_VERSION \ + RAPIDFIRE_CMD('F', '?') // - Firmware Version, Query +#define RAPIDFIRE_CMD_VOLTAGE RAPIDFIRE_CMD('V', '?') // - Voltage, Query +#define RAPIDFIRE_CMD_RSSI RAPIDFIRE_CMD('R', '?') // - RSSI, Query // Action #define RAPIDFIRE_CMD_SOUND_BUZZER RAPIDFIRE_CMD('S', '>') // - Buzzer @@ -57,4 +63,3 @@ #define RAPIDFIRE_CMD_SET_CHANNEL RAPIDFIRE_CMD('C', '=') #define RAPIDFIRE_CMD_SET_BAND RAPIDFIRE_CMD('B', '=') #define RAPIDFIRE_CMD_SET_RAPIDFIRE_MODE RAPIDFIRE_CMD('D', '=') - diff --git a/src/rapidfire.cpp b/src/rapidfire.cpp index fcca45fd..485f6d9d 100644 --- a/src/rapidfire.cpp +++ b/src/rapidfire.cpp @@ -1,5 +1,4 @@ #include "rapidfire.h" -#include "logging.h" static rapidFIRE_SPI rapidfire = rapidFIRE_SPI(PIN_CLK, PIN_MOSI, PIN_CS); @@ -10,90 +9,180 @@ void Rapidfire::Init() { rapidfire.begin(); + m_lastDisplayTime = 0; + DBGLN("Rapid Fire init"); } +bool Rapidfire::BufferIsClear(char *payload, int len) { + for (int i = 0; i < len; i++) { + if (payload[i] != 0) { + return false; + } + } + + return true; +} + void Rapidfire::SetOSD(mspPacket_t *packet) { int len = packet->payloadSize; + if (len < 4) { return; } - for (int i = 0; i < 4; i++) { //payload = [0x03, row, col, 0] - packet->readByte(); // skip the first 4 bytes - } + packet->readByte(); // skip + packet->readByte(); // row, not used + packet->readByte(); // col, not used + packet->readByte(); // skip + len -= 4; // subtract the first 4 bytes - if (len > MAX_LENGTH_TEXT) { - len = MAX_LENGTH_TEXT; + std::vector messageBuffer(len); + for (int i = 0; i < len; i++) { // Read entire payload into temporary buffer + messageBuffer[i] = packet->readByte(); } - for (int i = 0; i < len; i++) { - m_textBuffer[i] = packet->readByte(); + if (BufferIsClear(messageBuffer.data(), len)) { + return; } - DisplayTextBuffer(); + QueueMessageParts(messageBuffer, len, CountMessageParts(messageBuffer, len)); } -void Rapidfire::DisplayTextBuffer() { - if (m_displayStartMillis == 0) { - for (int i = 0; i < SPAM_COUNT; i++) { - rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::USERTEXT); - delay(DELAY_BETWEEN_SPI_PKT); +int Rapidfire::CountMessageParts(std::vector &buffer, int len) { + int numParts = 0; + for (int offset = 0; offset < len; offset += RAPIDFIRE_MAX_LENGTH_TEXT) { + int partSize = std::min(RAPIDFIRE_MAX_LENGTH_TEXT, len - offset); + if (partSize > 0 && !BufferIsClear(buffer.data() + offset, partSize)) { + numParts++; } } + return numParts; +} - // only send the text once to the rapidfire to avoid failure - rapidfire.setOSDUserText(m_textBuffer); // send the text to the rapidfire +void Rapidfire::QueueMessageParts(std::vector &buffer, int len, + int numParts) { + int currentPart = 0; + for (int offset = 0; offset < len; offset += RAPIDFIRE_MAX_LENGTH_TEXT) { + int partSize = std::min(RAPIDFIRE_MAX_LENGTH_TEXT, len - offset); - memset(m_textBuffer, 0, MAX_LENGTH_TEXT); + if (partSize <= 0 || BufferIsClear(buffer.data() + offset, partSize)) { + continue; + } - m_displayStartMillis = millis(); -} + std::string message; + if (numParts > 1) { + currentPart++; + message = "[" + std::to_string(currentPart) + "/" + + std::to_string(numParts) + "] "; + partSize = + std::min(partSize, RAPIDFIRE_MAX_LENGTH_TEXT - (int)message.length()); + } -void Rapidfire::SetRecordingState(uint8_t recordingState, uint16_t _delay){ - sprintf(m_textBuffer, "Is Recording %5d", recordingState); + message += std::string(buffer.begin() + offset, + buffer.begin() + offset + partSize); - DisplayTextBuffer(); + if (m_displayQueue.size() >= MAX_QUEUE_SIZE) { + m_displayQueue.pop(); + } + + m_displayQueue.push(message); + } +} + +void Rapidfire::SetRecordingState(uint8_t recordingState, uint16_t _delay) { + if (recordingState != 0) { + m_displayQueue.push("Recording: On"); + } else { + m_displayQueue.push("Recording: Off"); + } } void Rapidfire::SendHeadTrackingEnableCmd(bool enable) { - sprintf(m_textBuffer, "Head Tracking %5d", enable); + if (enable) { + m_displayQueue.push("Head Tracking: Enabled"); + } else { + m_displayQueue.push("Head Tracking: Disabled"); + } +} + +void Rapidfire::SendLinkTelemetry(uint8_t *rawCrsfPacket) { +#ifdef TELEMETRY_ON_OSD + + uint8_t rssi = rawCrsfPacket[3] * -1; + uint8_t lq = rawCrsfPacket[5] / 3; + uint8_t snr = rawCrsfPacket[6]; // actually int8_t + + m_displayQueue.push("RSSI: " + std::to_string(rssi) + "dB, LQ: " + + std::to_string(lq) + "%, SNR: " + std::to_string(snr)); +#endif +} + +void Rapidfire::SendBatteryTelemetry(uint8_t *rawCrsfPacket) { +#ifdef TELEMETRY_ON_OSD + + uint16_t voltage = ((uint16_t)rawCrsfPacket[3] << 8 | rawCrsfPacket[4]); + uint16_t amperage = ((uint16_t)rawCrsfPacket[5] << 8 | rawCrsfPacket[6]); + uint32_t mah = ((uint32_t)rawCrsfPacket[7] << 16 | + (uint32_t)rawCrsfPacket[8] << 8 | rawCrsfPacket[9]); + uint8_t percentage = rawCrsfPacket[10]; + + m_displayQueue.push("Battery: " + std::to_string(voltage) + "V, " + + std::to_string(amperage) + "A, " + std::to_string(mah) + + "mAh, " + std::to_string(percentage) + "%"); +#endif +} + +void Rapidfire::SendGpsTelemetry(crsf_packet_gps_t *packet) { +#ifdef TELEMETRY_ON_OSD - DisplayTextBuffer(); + int32_t rawLat = be32toh(packet->p.lat); // Convert to host byte order + int32_t rawLon = be32toh(packet->p.lon); // Convert to host byte order + + m_displayQueue.push("GPS: " + std::to_string(static_cast(rawLat)) + + ", " + std::to_string(static_cast(rawLon))); +#endif } void Rapidfire::SendIndexCmd(uint8_t index) { uint8_t newBand = index / 8 + 1; uint8_t newChannel = index % 8; + newChannel++; // ELRS channel is zero based, need to add 1 + rapidFIRE_SPI::BAND imrcBand; // convert ELRS band index to IMRC band index: switch (newBand) { - case 0x01: // Boscam A - imrcBand = rapidFIRE_SPI::BAND::BAND_A; - break; - case 0x02: // Boscam B - imrcBand = rapidFIRE_SPI::BAND::BAND_B; - break; - case 0x03: // Boscam E - imrcBand = rapidFIRE_SPI::BAND::BAND_E; - break; - case 0x04: // ImmersionRC/FatShark - imrcBand = rapidFIRE_SPI::BAND::BAND_F; - break; - case 0x05: // RaceBand - imrcBand = rapidFIRE_SPI::BAND::BAND_R; - break; - case 0x06: // LowRace - imrcBand = rapidFIRE_SPI::BAND::BAND_L; - break; - default: // ImmersionRC/FatShark - imrcBand = rapidFIRE_SPI::BAND::BAND_F; - break; + case 0x01: // Boscam A + imrcBand = rapidFIRE_SPI::BAND::BAND_A; + m_displayQueue.push("Band A:" + std::to_string(newChannel)); + break; + case 0x02: // Boscam B + imrcBand = rapidFIRE_SPI::BAND::BAND_B; + m_displayQueue.push("Band B:" + std::to_string(newChannel)); + break; + case 0x03: // Boscam E + imrcBand = rapidFIRE_SPI::BAND::BAND_E; + m_displayQueue.push("Band E:" + std::to_string(newChannel)); + break; + case 0x04: // ImmersionRC/FatShark + imrcBand = rapidFIRE_SPI::BAND::BAND_F; + m_displayQueue.push("Band F:" + std::to_string(newChannel)); + break; + case 0x05: // RaceBand + imrcBand = rapidFIRE_SPI::BAND::BAND_R; + m_displayQueue.push("RaceBand:" + std::to_string(newChannel)); + break; + case 0x06: // LowRace + imrcBand = rapidFIRE_SPI::BAND::BAND_L; + m_displayQueue.push("LowRace:" + std::to_string(newChannel)); + break; + default: // ImmersionRC/FatShark + imrcBand = rapidFIRE_SPI::BAND::BAND_F; + m_displayQueue.push("Band F:" + std::to_string(newChannel)); + break; } - newChannel++; // ELRS channel is zero based, need to add 1 - for (int i = 0; i < SPAM_COUNT; i++) { rapidfire.setBand(imrcBand); delay(DELAY_BETWEEN_SPI_PKT); @@ -102,19 +191,48 @@ void Rapidfire::SendIndexCmd(uint8_t index) { } } -void -Rapidfire::Loop(uint32_t now) -{ - ModuleBase::Loop(now); - - if (m_displayStartMillis != 0) { - if (now - m_displayStartMillis >= TIMEOUT_SET_OSD) - { - for (int i = 0; i < SPAM_COUNT; i++) { - rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::DEFAULTMODE); - delay(DELAY_BETWEEN_SPI_PKT); - } - m_displayStartMillis = 0; - } +void Rapidfire::ShowNextMessage() { + if (m_displayQueue.empty()) { + return; + } + + if (m_lastDisplayTime == 0) { // Only set OSD if not already set + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::USERTEXT); + delay(DELAY_BETWEEN_SPI_PKT); + } + } + + memset(m_textBuffer, 0, RAPIDFIRE_MAX_LENGTH_TEXT); + strncpy(m_textBuffer, m_displayQueue.front().c_str(), + RAPIDFIRE_MAX_LENGTH_TEXT); + m_displayQueue.pop(); + + rapidfire.setOSDUserText(m_textBuffer); + + m_lastDisplayTime = millis(); +} + +void Rapidfire::ClearOSD() { + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setOSDMode(rapidFIRE_SPI::OSDMODE::OFF); + delay(DELAY_BETWEEN_SPI_PKT); + } + + m_lastDisplayTime = 0; +} + +void Rapidfire::Loop(uint32_t now) { + ModuleBase::Loop(now); + + if (!m_displayQueue.empty()) { + // Show next message if display is inactive or enough time has passed + if (now - m_lastDisplayTime >= DISPLAY_INTERVAL) { + ShowNextMessage(); } + } else if (m_lastDisplayTime != 0 && + (now - m_lastDisplayTime >= OSD_TIMEOUT)) { + // Clear display after timeout if queue is empty and display is active + ClearOSD(); + } } diff --git a/src/rapidfire.h b/src/rapidfire.h index e8b5f433..6a754375 100644 --- a/src/rapidfire.h +++ b/src/rapidfire.h @@ -5,18 +5,19 @@ #include "msptypes.h" #include "rapidFIRE_SPI.h" #include "rapidFIRE_SPI_Protocol.h" -#include -#include -#define VRX_BOOT_DELAY 2000 // 2 seconds delay before sending any packets to rapidfire +#include "logging.h" +#include +#include +#include +#define VRX_BOOT_DELAY 2000 // 2 seconds delay before sending any packets #define DELAY_BETWEEN_SPI_PKT 100 // 100ms delay between each SPI packet - #define SPAM_COUNT 3 // rapidfire sometimes missed a pkt, so send 3x -#define MAX_LENGTH_TEXT 25 // max length of text to display on rapidfire osd - -#define TIMEOUT_SET_OSD 3000 // 3 seconds +#define MAX_QUEUE_SIZE 10 // max number of packets to queue for display +#define DISPLAY_INTERVAL 1000 // 1 seconds +#define OSD_TIMEOUT 2500 // 2.5 seconds class Rapidfire : public ModuleBase { public: @@ -28,9 +29,18 @@ class Rapidfire : public ModuleBase { void SendHeadTrackingEnableCmd(bool enable); void SetRecordingState(uint8_t recordingState, uint16_t delay); void SetOSD(mspPacket_t *packet); + void SendLinkTelemetry(uint8_t *rawCrsfPacket); + void SendBatteryTelemetry(uint8_t *rawCrsfPacket); + void SendGpsTelemetry(crsf_packet_gps_t *packet); private: - uint32_t m_displayStartMillis = 0; - char m_textBuffer[MAX_LENGTH_TEXT]; - void DisplayTextBuffer(); + uint32_t m_lastDisplayTime = 0; + char m_textBuffer[RAPIDFIRE_MAX_LENGTH_TEXT]; + std::queue m_displayQueue; + + void ShowNextMessage(); + void ClearOSD(); + bool BufferIsClear(char *payload, int len); + int CountMessageParts(std::vector &buffer, int len); + void QueueMessageParts(std::vector &buffer, int len, int numParts); };