diff --git a/lib/rapidfire_SPI/rapidFIRE_SPI.cpp b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp new file mode 100644 index 00000000..fcba8192 --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.cpp @@ -0,0 +1,213 @@ +// 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(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 (size_t i = 0; i < len; i++) { + data[i] = spi->transfer(0); + } + + spi->end(); + spi->setHwCs(false); + + byte csum = recv_len; + for (size_t i = 0; i < len; ++i) { + csum += data[i]; + } + + 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 (size_t 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); + + 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..f8e623ce --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI.h @@ -0,0 +1,105 @@ +// 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 = -1; + + 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 # Seems to have compatibility issues + // with OSD UserText + 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..eb33ab84 --- /dev/null +++ b/lib/rapidfire_SPI/rapidFIRE_SPI_Protocol.h @@ -0,0 +1,65 @@ +// 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 + +#define RAPIDFIRE_MAX_LENGTH_TEXT \ + 25 // max length of text to display on rapidfire osd + +// 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..485f6d9d 100644 --- a/src/rapidfire.cpp +++ b/src/rapidfire.cpp @@ -1,225 +1,238 @@ #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(); + + m_lastDisplayTime = 0; -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); +bool Rapidfire::BufferIsClear(char *payload, int len) { + for (int i = 0; i < len; i++) { + if (payload[i] != 0) { + return false; + } + } + + return true; } -void -Rapidfire::SendIndexCmd(uint8_t index) -{ - uint8_t newBand = index / 8 + 1; - uint8_t newChannel = index % 8; +void Rapidfire::SetOSD(mspPacket_t *packet) { + int len = packet->payloadSize; + + if (len < 4) { + return; + } + + packet->readByte(); // skip + packet->readByte(); // row, not used + packet->readByte(); // col, not used + packet->readByte(); // skip + + len -= 4; // subtract the first 4 bytes + + std::vector messageBuffer(len); + for (int i = 0; i < len; i++) { // Read entire payload into temporary buffer + messageBuffer[i] = packet->readByte(); + } - SendBandCmd(newBand); - delay(100); - SendChannelCmd(newChannel); + if (BufferIsClear(messageBuffer.data(), len)) { + return; + } + + QueueMessageParts(messageBuffer, len, CountMessageParts(messageBuffer, len)); } -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); +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; } -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; +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); + + if (partSize <= 0 || BufferIsClear(buffer.data() + offset, partSize)) { + continue; + } + + 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()); } - 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); + message += std::string(buffer.begin() + offset, + buffer.begin() + offset + partSize); + + if (m_displayQueue.size() >= MAX_QUEUE_SIZE) { + m_displayQueue.pop(); } + + m_displayQueue.push(message); + } } -void -Rapidfire::SendSPI(uint8_t* buf, uint8_t bufLen) -{ - if (!SPIModeEnabled) EnableSPIMode(); - - uint32_t periodMicroSec = 1000000 / BIT_BANG_FREQ; - - pinMode(PIN_MOSI, OUTPUT); - pinMode(PIN_CLK, OUTPUT); - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_MOSI, LOW); - digitalWrite(PIN_CLK, LOW); - digitalWrite(PIN_CS, HIGH); - - delayMicroseconds(periodMicroSec); - - digitalWrite(PIN_CS, LOW); - delay(100); - - // debug code for printing SPI pkt - for (int i = 0; i < bufLen; ++i) - { - uint8_t bufByte = buf[i]; - - DBG("%x", bufByte); - DBG(","); - - 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; - } +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) { + 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 + + 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; + 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; + } + + for (int i = 0; i < SPAM_COUNT; i++) { + rapidfire.setBand(imrcBand); + delay(DELAY_BETWEEN_SPI_PKT); + rapidfire.setChannel(newChannel); + delay(DELAY_BETWEEN_SPI_PKT); + } +} + +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); } - DBGLN(""); + } + + memset(m_textBuffer, 0, RAPIDFIRE_MAX_LENGTH_TEXT); + strncpy(m_textBuffer, m_displayQueue.front().c_str(), + RAPIDFIRE_MAX_LENGTH_TEXT); + m_displayQueue.pop(); - digitalWrite(PIN_MOSI, LOW); - digitalWrite(PIN_CLK, LOW); - digitalWrite(PIN_CS, HIGH); - delay(100); + rapidfire.setOSDUserText(m_textBuffer); - pinMode(PIN_MOSI, INPUT); - pinMode(PIN_CLK, INPUT); - pinMode(PIN_CS, INPUT); + m_lastDisplayTime = millis(); } -// CRC function for IMRC rapidfire API -// Input: byte array, array length -// Output: crc byte -uint8_t -Rapidfire::crc8(uint8_t* buf, uint8_t bufLen) -{ - uint32_t sum = 0; - for (uint8_t i = 0; i < bufLen; ++i) - { - sum += buf[i]; +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(); } - return sum & 0xFF; } diff --git a/src/rapidfire.h b/src/rapidfire.h index b70b3ca8..6a754375 100644 --- a/src/rapidfire.h +++ b/src/rapidfire.h @@ -1,31 +1,46 @@ #pragma once #include "module_base.h" -#include +#include "msp.h" +#include "msptypes.h" +#include "rapidFIRE_SPI.h" +#include "rapidFIRE_SPI_Protocol.h" -#define VRX_BOOT_DELAY 2000 +#include "logging.h" +#include +#include +#include -#define BIT_BANG_FREQ 1000 -#define SPAM_COUNT 3 +#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 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 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 -{ +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); + void SendLinkTelemetry(uint8_t *rawCrsfPacket); + void SendBatteryTelemetry(uint8_t *rawCrsfPacket); + void SendGpsTelemetry(crsf_packet_gps_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_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); };