From 5d2598c6189cfd54bb91170a611de1bd2246d5fb Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 10:05:35 +0100 Subject: [PATCH 01/11] vtx tramp init commit --- lib/Espfc/src/Connect/MspProcessor.cpp | 64 +++++++++++++++++------ lib/Espfc/src/Connect/Vtx.cpp | 71 +++++++++++++++++++++----- lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/ModelConfig.h | 1 + lib/Espfc/src/SerialManager.cpp | 11 ++++ 5 files changed, 119 insertions(+), 30 deletions(-) diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index f877b2fc..a14d72e3 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -151,6 +151,40 @@ static uint16_t toIbatCurrent(float current) return constrain(lrintf(current * 100.0f), -32000, 32000); } +static uint8_t mapPowerToTramp(uint8_t power) { + switch (power) { + case 25: + return 0; + case 100: + return 1; + case 200: + return 2; + case 400: + return 3; + case 600: + return 4; + default: + return 0; // Default to the lowest power if unknown + } +} + +uint8_t mapTrampToPower(uint8_t powerIndex) { + switch (powerIndex) { + case 0: + return 25; // 25 mW + case 1: + return 100; // 100 mW + case 2: + return 200; // 200 mW + case 3: + return 400; // 400 mW + case 4: + return 600; // 600 mW + default: + return 25; // Default to 25 mW if unknown + } +} + constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; } @@ -1403,34 +1437,33 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(0); // ready r.writeU8(0); // low power disarm } else { - r.writeU8(3 /* SMARTAUDIO */); // vtx type unknown - r.writeU8(_model.config.vtx.band); // band - r.writeU8(_model.config.vtx.channel); // channel - r.writeU8(_model.config.vtx.power); // power - r.writeU8(0); // status (looks like 1 means pit mode :shrug:) + r.writeU8(_model.config.vtx.protocol); // vtx type + r.writeU8(_model.config.vtx.band); // band + r.writeU8(_model.config.vtx.channel); // channel + r.writeU8(_model.config.vtx.power); // power + r.writeU8(0); // status (1 indicates pit mode) r.writeU16(0); // freq r.writeU8(1); // ready - r.writeU8(_model.config.vtx.lowPowerDisarm); // low power disarm + r.writeU8(_model.config.vtx.lowPowerDisarm); // low power disarm } - // 1.42 + // API version 1.42 r.writeU16(0); // pit mode freq r.writeU8(0); // vtx table available (no) r.writeU8(0); // vtx table bands r.writeU8(0); // vtx table channels r.writeU8(0); // vtx power levels break; - + case MSP_SET_VTX_CONFIG: { uint16_t freq = m.readU16(); if (freq <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel - //const uint8_t newBand = (freq / 8) + 1; - //const uint8_t newChannel = (freq % 8) + 1; + // Extract band and channel if needed } if (m.remain() >= 2) { - _model.config.vtx.power = m.readU8(); - /*const uint8_t newPitmode = */m.readU8(); + _model.config.vtx.power = m.readU8(); + m.readU8(); // Read pitmode but not used } if (m.remain()) { @@ -1439,21 +1472,18 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD // API version 1.42 - this parameter kept separate since clients may already be supplying if (m.remain() >= 2) { - /*const uint16_t pitModeFreq = */m.readU16(); + m.readU16(); // Read pitModeFreq but not used } // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency if (m.remain() >= 4) { - // Added standalone values for band, channel and frequency to move - // away from the flawed encoded combined method originally implemented. _model.config.vtx.band = m.readU8(); _model.config.vtx.channel = m.readU8(); - /*uint16_t newFreq = */m.readU16(); + m.readU16(); // Read newFreq but not used } } break; - case MSP_SET_ARMING_DISABLED: { const uint8_t cmd = m.readU8(); diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index c972f5ca..ba93ef55 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -27,6 +27,7 @@ static unsigned char crc8tab[256] = 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9 }; + static uint8_t crc8(const uint8_t * ptr, uint8_t len) { uint8_t crc = 0; @@ -36,6 +37,14 @@ static uint8_t crc8(const uint8_t * ptr, uint8_t len) return crc; } +struct TrampCommand { + uint8_t header[2] = {0x0F, 0x00}; // Header bytes + uint8_t command; // Command identifier + uint8_t payload[12] = {0}; // Data payload + uint8_t crc; // CRC byte + uint8_t terminator = 0x00; // Terminator byte +}; + namespace Espfc::Connect { int Vtx::begin(Device::SerialDevice * serial) @@ -43,6 +52,16 @@ int Vtx::begin(Device::SerialDevice * serial) _serial = serial; _timer.setRate(300); + if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp + { + // Send initialization command + TrampCommand initCmd; + initCmd.command = 'r'; // 'r' for reset/init + initCmd.crc = crc8(reinterpret_cast(&initCmd), sizeof(initCmd) - 2); + _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); + _serial->flush(); + } + _state = State::INIT; return 1; } @@ -81,23 +100,51 @@ int Vtx::update() int Vtx::setChannel() { - uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(dummyByte, 1); - _serial->write(vtxCommand, 6); - _serial->flush(); - + uint8_t vtxCommand[6]; + if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio + { + uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; + vtxCommand[5] = crc8(vtxCommand, 5); + _serial->write(dummyByte, 1); + _serial->write(vtxCommand, 6); + _serial->flush(); + } + else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp + { + vtxCommand[0] = 0x0F; + vtxCommand[1] = 0x55; + vtxCommand[2] = 0x00; + vtxCommand[3] = 0x00; + vtxCommand[4] = (_model.config.vtx.band - 1) * 8 + (_model.config.vtx.channel - 1); + vtxCommand[5] = crc8(vtxCommand, 5); + _serial->write(vtxCommand, 6); + _serial->flush(); + } return 1; } int Vtx::setPower() { - uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(dummyByte, 1); - _serial->write(vtxCommand, 6); - _serial->flush(); - + uint8_t vtxCommand[6]; + if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio + { + uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, !_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED) ? _model.config.vtx.power - 1 : 0 }; + vtxCommand[5] = crc8(vtxCommand, 5); + _serial->write(dummyByte, 1); + _serial->write(vtxCommand, 6); + _serial->flush(); + } + else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp + { + vtxCommand[0] = 0x0F; + vtxCommand[1] = 0x56; + vtxCommand[2] = 0x00; + vtxCommand[3] = 0x00; + vtxCommand[4] = (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power : 0; + vtxCommand[5] = crc8(vtxCommand, 5); + _serial->write(vtxCommand, 6); + _serial->flush(); + } return 1; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 5b2e4c7c..a6cd1b83 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -393,7 +393,7 @@ class Model // configure serial ports constexpr uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | - SERIAL_FUNCTION_GPS | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT | SERIAL_FUNCTION_TELEMETRY_IBUS | SERIAL_FUNCTION_VTX_SMARTAUDIO; + SERIAL_FUNCTION_GPS | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT | SERIAL_FUNCTION_TELEMETRY_IBUS | SERIAL_FUNCTION_VTX_SMARTAUDIO | SERIAL_FUNCTION_VTX_TRAMP; uint32_t featureAllowMask = FEATURE_RX_PPM | FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_SOFTSERIAL | FEATURE_GPS | FEATURE_TELEMETRY | FEATURE_RX_SPI;// | FEATURE_AIRMODE; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 311816e0..d4cb167c 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -653,6 +653,7 @@ struct ControllerConfig struct VtxConfig { + uint8_t protocol = 0; // 0: None, 3: SmartAudio, 4: Tramp uint8_t channel = 0x8; uint8_t band = 0x1; uint8_t power = 0; diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index f4805d64..02edcb60 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -105,6 +105,13 @@ int SerialManager::begin() sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; sdc.data_bits = 8; } + else if(spc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) + { + sdc.baud = 9600; + sdc.parity = SDC_SERIAL_PARITY_NONE; + sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; + sdc.data_bits = 8; + } if(!sdc.baud) { @@ -126,6 +133,10 @@ int SerialManager::begin() { _vtx.begin(port); } + if(spc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) + { + _vtx.begin(port); + } if(spc.functionMask & SERIAL_FUNCTION_GPS) { _gps.begin(port, sdc.baud); From fcae2a8270194c65230e9d6839dcff1c67067918 Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 10:14:08 +0100 Subject: [PATCH 02/11] remove obsolete --- lib/Espfc/src/Connect/MspProcessor.cpp | 34 -------------------------- 1 file changed, 34 deletions(-) diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index a14d72e3..af262668 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -151,40 +151,6 @@ static uint16_t toIbatCurrent(float current) return constrain(lrintf(current * 100.0f), -32000, 32000); } -static uint8_t mapPowerToTramp(uint8_t power) { - switch (power) { - case 25: - return 0; - case 100: - return 1; - case 200: - return 2; - case 400: - return 3; - case 600: - return 4; - default: - return 0; // Default to the lowest power if unknown - } -} - -uint8_t mapTrampToPower(uint8_t powerIndex) { - switch (powerIndex) { - case 0: - return 25; // 25 mW - case 1: - return 100; // 100 mW - case 2: - return 200; // 200 mW - case 3: - return 400; // 400 mW - case 4: - return 600; // 600 mW - default: - return 25; // Default to 25 mW if unknown - } -} - constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; } From e2d5742225058af6ecf15fc65b7a20c7970442af Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 10:21:00 +0100 Subject: [PATCH 03/11] fix --- lib/Espfc/src/Connect/Vtx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index ba93ef55..4f8c5273 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -100,7 +100,6 @@ int Vtx::update() int Vtx::setChannel() { - uint8_t vtxCommand[6]; if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio { uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; @@ -111,6 +110,7 @@ int Vtx::setChannel() } else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp { + uint8_t vtxCommand[6]; vtxCommand[0] = 0x0F; vtxCommand[1] = 0x55; vtxCommand[2] = 0x00; @@ -125,7 +125,6 @@ int Vtx::setChannel() int Vtx::setPower() { - uint8_t vtxCommand[6]; if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio { uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, !_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED) ? _model.config.vtx.power - 1 : 0 }; @@ -136,6 +135,7 @@ int Vtx::setPower() } else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp { + uint8_t vtxCommand[6]; vtxCommand[0] = 0x0F; vtxCommand[1] = 0x56; vtxCommand[2] = 0x00; From b77984693a7dba64700cb7ca07231bc03572556f Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 10:24:30 +0100 Subject: [PATCH 04/11] revert --- lib/Espfc/src/Connect/Vtx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index 4f8c5273..f29bff54 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -127,7 +127,7 @@ int Vtx::setPower() { if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio { - uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, !_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED) ? _model.config.vtx.power - 1 : 0 }; + uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; vtxCommand[5] = crc8(vtxCommand, 5); _serial->write(dummyByte, 1); _serial->write(vtxCommand, 6); From 86cb6e164bb1244ddc2396c68191a1970d3a398b Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 11:23:37 +0100 Subject: [PATCH 05/11] more fixes --- lib/Espfc/src/Connect/MspProcessor.cpp | 13 +++++++----- lib/Espfc/src/Connect/Vtx.cpp | 28 +++++++++++++------------- lib/Espfc/src/SerialManager.cpp | 4 ++++ 3 files changed, 26 insertions(+), 19 deletions(-) diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index af262668..ac03fe2c 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -1424,12 +1424,13 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { uint16_t freq = m.readU16(); if (freq <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel - // Extract band and channel if needed + // const uint8_t newBand = (freq / 8) + 1; + // const uint8_t newChannel = (freq % 8) + 1; } if (m.remain() >= 2) { - _model.config.vtx.power = m.readU8(); - m.readU8(); // Read pitmode but not used + _model.config.vtx.power = m.readU8(); + /*const uint8_t newPitmode = */m.readU8(); } if (m.remain()) { @@ -1438,14 +1439,16 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD // API version 1.42 - this parameter kept separate since clients may already be supplying if (m.remain() >= 2) { - m.readU16(); // Read pitModeFreq but not used + /*const uint16_t pitModeFreq = */m.readU16(); } // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency if (m.remain() >= 4) { + // Added standalone values for band, channel and frequency to move + // away from the flawed encoded combined method originally implemented. _model.config.vtx.band = m.readU8(); _model.config.vtx.channel = m.readU8(); - m.readU16(); // Read newFreq but not used + /*uint16_t newFreq = */m.readU16(); } } break; diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index f29bff54..5c8bac89 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -52,16 +52,6 @@ int Vtx::begin(Device::SerialDevice * serial) _serial = serial; _timer.setRate(300); - if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp - { - // Send initialization command - TrampCommand initCmd; - initCmd.command = 'r'; // 'r' for reset/init - initCmd.crc = crc8(reinterpret_cast(&initCmd), sizeof(initCmd) - 2); - _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); - _serial->flush(); - } - _state = State::INIT; return 1; } @@ -72,6 +62,16 @@ int Vtx::update() switch (_state) { case State::INIT: + _model.config.vtx.protocol = VTXDEV_TRAMP; + if (_model.config.vtx.protocol == VTXDEV_TRAMP) + { + // Send initialization command + TrampCommand initCmd; + initCmd.command = 'r'; // 'r' for reset/init + initCmd.crc = crc8(reinterpret_cast(&initCmd), sizeof(initCmd) - 2); + _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); + _serial->flush(); + } _state = State::SET_CHANNEL; _model.state.vtx.active = true; break; @@ -100,7 +100,7 @@ int Vtx::update() int Vtx::setChannel() { - if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio + if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) { uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; vtxCommand[5] = crc8(vtxCommand, 5); @@ -108,7 +108,7 @@ int Vtx::setChannel() _serial->write(vtxCommand, 6); _serial->flush(); } - else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp + else if (_model.config.vtx.protocol == VTXDEV_TRAMP) { uint8_t vtxCommand[6]; vtxCommand[0] = 0x0F; @@ -125,7 +125,7 @@ int Vtx::setChannel() int Vtx::setPower() { - if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) // SmartAudio + if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) { uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; vtxCommand[5] = crc8(vtxCommand, 5); @@ -133,7 +133,7 @@ int Vtx::setPower() _serial->write(vtxCommand, 6); _serial->flush(); } - else if (_model.config.vtx.protocol == VTXDEV_TRAMP) // IRC Tramp + else if (_model.config.vtx.protocol == VTXDEV_TRAMP) { uint8_t vtxCommand[6]; vtxCommand[0] = 0x0F; diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index 02edcb60..f5394b25 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -176,6 +176,10 @@ int FAST_CODE_ATTR SerialManager::update() { _vtx.update(); } + if(sc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) + { + _vtx.update(); + } if(sc.functionMask & SERIAL_FUNCTION_GPS) { _gps.update(); From e978b77d0466a1e08a7f6711cf2adb6d174545f6 Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 11:26:40 +0100 Subject: [PATCH 06/11] undemo --- lib/Espfc/src/Connect/Vtx.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index 5c8bac89..86be7e2b 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -62,7 +62,6 @@ int Vtx::update() switch (_state) { case State::INIT: - _model.config.vtx.protocol = VTXDEV_TRAMP; if (_model.config.vtx.protocol == VTXDEV_TRAMP) { // Send initialization command From 1908504277b15de2d39e16e54b05e5843b418926 Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 21 Mar 2025 18:56:09 +0100 Subject: [PATCH 07/11] some progress --- lib/Espfc/src/Connect/MspProcessor.cpp | 2 +- lib/Espfc/src/Connect/Vtx.cpp | 150 ------------------ lib/Espfc/src/Connect/VtxSmartAudio.cpp | 69 ++++++++ .../Connect/{Vtx.hpp => VtxSmartAudio.hpp} | 26 +-- lib/Espfc/src/Connect/VtxTramp.cpp | 89 +++++++++++ lib/Espfc/src/Connect/VtxTramp.hpp | 40 +++++ lib/Espfc/src/ModelConfig.h | 1 - lib/Espfc/src/ModelState.h | 1 + lib/Espfc/src/SerialManager.cpp | 16 +- lib/Espfc/src/SerialManager.h | 6 +- lib/Espfc/src/Vtx.h | 23 +++ 11 files changed, 242 insertions(+), 181 deletions(-) delete mode 100644 lib/Espfc/src/Connect/Vtx.cpp create mode 100644 lib/Espfc/src/Connect/VtxSmartAudio.cpp rename lib/Espfc/src/Connect/{Vtx.hpp => VtxSmartAudio.hpp} (50%) create mode 100644 lib/Espfc/src/Connect/VtxTramp.cpp create mode 100644 lib/Espfc/src/Connect/VtxTramp.hpp create mode 100644 lib/Espfc/src/Vtx.h diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index ac03fe2c..00cc7c92 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -1403,7 +1403,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(0); // ready r.writeU8(0); // low power disarm } else { - r.writeU8(_model.config.vtx.protocol); // vtx type + r.writeU8(_model.state.vtx.protocol); // vtx type r.writeU8(_model.config.vtx.band); // band r.writeU8(_model.config.vtx.channel); // channel r.writeU8(_model.config.vtx.power); // power diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp deleted file mode 100644 index 86be7e2b..00000000 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include "Vtx.hpp" - -static const uint8_t dummyByte[] = { 0x00 }; - -/* CRC8 implementation with polynom = x -7+ x -6+ x -4+ x -2+ x -0 (0xD5) */ -static unsigned char crc8tab[256] = -{ - 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, - 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, - 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, - 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, - 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, - 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, - 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, - 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, - 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, - 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, - 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, - 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, - 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, - 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, - 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, - 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9 -}; - -static uint8_t crc8(const uint8_t * ptr, uint8_t len) -{ - uint8_t crc = 0; - for (uint8_t i = 0; i < len; i++) { - crc = crc8tab[crc ^ *ptr++]; -} -return crc; -} - -struct TrampCommand { - uint8_t header[2] = {0x0F, 0x00}; // Header bytes - uint8_t command; // Command identifier - uint8_t payload[12] = {0}; // Data payload - uint8_t crc; // CRC byte - uint8_t terminator = 0x00; // Terminator byte -}; - -namespace Espfc::Connect { - -int Vtx::begin(Device::SerialDevice * serial) -{ - _serial = serial; - _timer.setRate(300); - - _state = State::INIT; - return 1; -} - -int Vtx::update() -{ - if (!_timer.check()) return 1; - switch (_state) - { - case State::INIT: - if (_model.config.vtx.protocol == VTXDEV_TRAMP) - { - // Send initialization command - TrampCommand initCmd; - initCmd.command = 'r'; // 'r' for reset/init - initCmd.crc = crc8(reinterpret_cast(&initCmd), sizeof(initCmd) - 2); - _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); - _serial->flush(); - } - _state = State::SET_CHANNEL; - _model.state.vtx.active = true; - break; - case State::SET_POWER: - setPower(); - _state = State::IDLE; - break; - case State::SET_CHANNEL: - setChannel(); - _state = State::SET_POWER; - break; - case State::IDLE: - if (_model.isModeActive(MODE_ARMED) != _armed) - { - _armed = !_armed; - _state = State::SET_POWER; - } - break; - case State::INACTIVE: - default: - break; - } - - return 1; -} - -int Vtx::setChannel() -{ - if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) - { - uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(dummyByte, 1); - _serial->write(vtxCommand, 6); - _serial->flush(); - } - else if (_model.config.vtx.protocol == VTXDEV_TRAMP) - { - uint8_t vtxCommand[6]; - vtxCommand[0] = 0x0F; - vtxCommand[1] = 0x55; - vtxCommand[2] = 0x00; - vtxCommand[3] = 0x00; - vtxCommand[4] = (_model.config.vtx.band - 1) * 8 + (_model.config.vtx.channel - 1); - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(vtxCommand, 6); - _serial->flush(); - } - return 1; -} - -int Vtx::setPower() -{ - if (_model.config.vtx.protocol == VTXDEV_SMARTAUDIO) - { - uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(dummyByte, 1); - _serial->write(vtxCommand, 6); - _serial->flush(); - } - else if (_model.config.vtx.protocol == VTXDEV_TRAMP) - { - uint8_t vtxCommand[6]; - vtxCommand[0] = 0x0F; - vtxCommand[1] = 0x56; - vtxCommand[2] = 0x00; - vtxCommand[3] = 0x00; - vtxCommand[4] = (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power : 0; - vtxCommand[5] = crc8(vtxCommand, 5); - _serial->write(vtxCommand, 6); - _serial->flush(); - } - return 1; -} - -} diff --git a/lib/Espfc/src/Connect/VtxSmartAudio.cpp b/lib/Espfc/src/Connect/VtxSmartAudio.cpp new file mode 100644 index 00000000..9ee62af6 --- /dev/null +++ b/lib/Espfc/src/Connect/VtxSmartAudio.cpp @@ -0,0 +1,69 @@ +#include "VtxSmartAudio.hpp" +#include "Utils/Crc.hpp" + +static const uint8_t dummyByte[] = { 0x00 }; + +namespace Espfc::Connect { + +int VtxSmartAudio::begin(Device::SerialDevice * serial) +{ + _serial = serial; + _timer.setRate(300); + + _state = State::INIT; + return 1; +} + +int VtxSmartAudio::update() +{ + if (!_timer.check()) return 1; + switch (_state) + { + case State::INIT: + _state = State::SET_CHANNEL; + _model.state.vtx.active = true; + break; + case State::SET_POWER: + setPower(); + _state = State::IDLE; + break; + case State::SET_CHANNEL: + setChannel(); + _state = State::SET_POWER; + break; + case State::IDLE: + if (_model.isModeActive(MODE_ARMED) != _armed) + { + _armed = !_armed; + _state = State::SET_POWER; + } + break; + case State::INACTIVE: + default: + break; + } + + return 1; +} + +int VtxSmartAudio::setChannel() +{ + uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x07, 0x01, (uint8_t)((_model.config.vtx.band -1)*8 + _model.config.vtx.channel - 1) }; + vtxCommand[5] = Utils::crc8_dvb_s2(vtxCommand[5], reinterpret_cast(&vtxCommand), 5); + _serial->write(dummyByte, 1); + _serial->write(vtxCommand, 6); + _serial->flush(); + return 1; +} + +int VtxSmartAudio::setPower() +{ + uint8_t vtxCommand[6] = { 0xAA, 0x55, 0x05, 0x01, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; + vtxCommand[5] = Utils::crc8_dvb_s2(vtxCommand[5], reinterpret_cast(&vtxCommand), 5); + _serial->write(dummyByte, 1); + _serial->write(vtxCommand, 6); + _serial->flush(); + return 1; +} + +} diff --git a/lib/Espfc/src/Connect/Vtx.hpp b/lib/Espfc/src/Connect/VtxSmartAudio.hpp similarity index 50% rename from lib/Espfc/src/Connect/Vtx.hpp rename to lib/Espfc/src/Connect/VtxSmartAudio.hpp index a6ae2956..b919d659 100644 --- a/lib/Espfc/src/Connect/Vtx.hpp +++ b/lib/Espfc/src/Connect/VtxSmartAudio.hpp @@ -3,37 +3,20 @@ #include "Device/SerialDevice.h" #include "Utils/Timer.h" #include "Model.h" +#include "Vtx.h" namespace Espfc::Connect { -enum VtxDeviceType { - VTXDEV_UNSUPPORTED = 0, // reserved for MSP - VTXDEV_RTC6705 = 1, - // 2 reserved - VTXDEV_SMARTAUDIO = 3, - VTXDEV_TRAMP = 4, - VTXDEV_MSP = 5, - VTXDEV_UNKNOWN = 0xFF, -}; - -enum State { - INACTIVE, - INIT, - SET_POWER, - SET_CHANNEL, - IDLE, -}; - -class Vtx +class VtxSmartAudio { public: - Vtx(Model& model): _serial(NULL), _model(model) {} + VtxSmartAudio(Model& model): _serial(NULL), _model(model) {} int begin(Device::SerialDevice * serial); int update(); int setChannel(); int setPower(); - Connect::VtxDeviceType type; + Connect::VtxDeviceType type = Connect::VTXDEV_SMARTAUDIO; private: Device::SerialDevice* _serial; @@ -43,4 +26,5 @@ class Vtx Utils::Timer _timer; }; + } diff --git a/lib/Espfc/src/Connect/VtxTramp.cpp b/lib/Espfc/src/Connect/VtxTramp.cpp new file mode 100644 index 00000000..1d1befb2 --- /dev/null +++ b/lib/Espfc/src/Connect/VtxTramp.cpp @@ -0,0 +1,89 @@ +#include "VtxTramp.hpp" +#include "Utils/Crc.hpp" + +static const uint8_t dummyByte[] = { 0x00 }; + +namespace Espfc::Connect { + +int VtxTramp::begin(Device::SerialDevice * serial) +{ + _serial = serial; + _timer.setRate(300); + + _state = State::INIT; + return 1; +} + +int VtxTramp::initTramp() +{ + // Send initialization command + TrampCommand initCmd; + initCmd.command = 'r'; // 'r' for reset/init + initCmd.crc = Utils::crc8_dvb_s2(0, reinterpret_cast(&initCmd), sizeof(initCmd) - 2); + _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); + _serial->flush(); + return 1; +} + +int VtxTramp::update() +{ + if (!_timer.check()) return 1; + switch (_state) + { + case State::INIT: + initTramp(); + _state = State::SET_CHANNEL; + _model.state.vtx.active = true; + break; + case State::SET_POWER: + setPower(); + _state = State::IDLE; + break; + case State::SET_CHANNEL: + setChannel(); + _state = State::SET_POWER; + break; + case State::IDLE: + if (_model.isModeActive(MODE_ARMED) != _armed) + { + _armed = !_armed; + _state = State::SET_POWER; + } + break; + case State::INACTIVE: + default: + break; + } + + return 1; +} + +int VtxTramp::setChannel() +{ + uint8_t vtxCommand[6]; + vtxCommand[0] = 0x0F; + vtxCommand[1] = 0x55; + vtxCommand[2] = 0x00; + vtxCommand[3] = 0x00; + vtxCommand[4] = (_model.config.vtx.band - 1) * 8 + (_model.config.vtx.channel - 1); + vtxCommand[5] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 5); + _serial->write(vtxCommand, 6); + _serial->flush(); + return 1; +} + +int VtxTramp::setPower() +{ + uint8_t vtxCommand[6]; + vtxCommand[0] = 0x0F; + vtxCommand[1] = 0x56; + vtxCommand[2] = 0x00; + vtxCommand[3] = 0x00; + vtxCommand[4] = (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power : 0; + vtxCommand[5] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 5); + _serial->write(vtxCommand, 6); + _serial->flush(); + return 1; +} + +} diff --git a/lib/Espfc/src/Connect/VtxTramp.hpp b/lib/Espfc/src/Connect/VtxTramp.hpp new file mode 100644 index 00000000..2785097a --- /dev/null +++ b/lib/Espfc/src/Connect/VtxTramp.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include "Device/SerialDevice.h" +#include "Utils/Timer.h" +#include "Model.h" +#include "Vtx.h" + +namespace Espfc::Connect { + +class VtxTramp +{ + public: + VtxTramp(Model& model): _serial(NULL), _model(model) {} + + int begin(Device::SerialDevice * serial); + int initTramp(); + int update(); + int setChannel(); + int setPower(); + Connect::VtxDeviceType type = Connect::VTXDEV_TRAMP; + + private: + Device::SerialDevice* _serial; + Model& _model; + State _state = State::INACTIVE; + bool _armed = false; + Utils::Timer _timer; +}; + + +struct TrampCommand { + uint8_t header[2] = {0x0F, 0x00}; // Header bytes + uint8_t command; // Command identifier + uint8_t payload[12] = {0}; // Data payload + uint8_t crc; // CRC byte + uint8_t terminator = 0x00; // Terminator byte +}; + + +} diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index d4cb167c..311816e0 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -653,7 +653,6 @@ struct ControllerConfig struct VtxConfig { - uint8_t protocol = 0; // 0: None, 3: SmartAudio, 4: Tramp uint8_t channel = 0x8; uint8_t band = 0x1; uint8_t power = 0; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index ce25c2fe..fa356501 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -328,6 +328,7 @@ struct ModeState struct VtxState { uint8_t active = false; + uint8_t protocol = 0; }; enum GpsDeviceVersion diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index f5394b25..819544c5 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -21,8 +21,8 @@ namespace Espfc { -SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _current(0), _msp(model), _cli(model), _vtx(model), - _telemetry(telemetry), _gps(model) +SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _current(0), _msp(model), _cli(model), + _vtx_smartaudio(model), _vtx_tramp(model), _telemetry(telemetry), _gps(model) #ifdef ESPFC_SERIAL_SOFT_0_WIFI , _wireless(model) #endif @@ -104,6 +104,8 @@ int SerialManager::begin() sdc.parity = SDC_SERIAL_PARITY_NONE; sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; sdc.data_bits = 8; + + _model.state.vtx.protocol = _vtx_smartaudio.type; } else if(spc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) { @@ -111,6 +113,8 @@ int SerialManager::begin() sdc.parity = SDC_SERIAL_PARITY_NONE; sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; sdc.data_bits = 8; + + _model.state.vtx.protocol = _vtx_tramp.type; } if(!sdc.baud) @@ -131,11 +135,11 @@ int SerialManager::begin() } if(spc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO) { - _vtx.begin(port); + _vtx_smartaudio.begin(port); } if(spc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) { - _vtx.begin(port); + _vtx_tramp.begin(port); } if(spc.functionMask & SERIAL_FUNCTION_GPS) { @@ -174,11 +178,11 @@ int FAST_CODE_ATTR SerialManager::update() } if(sc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO) { - _vtx.update(); + _vtx_smartaudio.update(); } if(sc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) { - _vtx.update(); + _vtx_tramp.update(); } if(sc.functionMask & SERIAL_FUNCTION_GPS) { diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index f839928a..db298d8f 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -3,7 +3,8 @@ #include "Model.h" #include "Device/SerialDevice.h" #include "Connect/MspProcessor.hpp" -#include "Connect/Vtx.hpp" +#include "Connect/VtxSmartAudio.hpp" +#include "Connect/VtxTramp.hpp" #include "Connect/Cli.hpp" #include "TelemetryManager.h" #include "Output/OutputIBUS.hpp" @@ -37,7 +38,8 @@ class SerialManager Connect::MspProcessor _msp; Connect::Cli _cli; - Connect::Vtx _vtx; + Connect::VtxSmartAudio _vtx_smartaudio; + Connect::VtxTramp _vtx_tramp; TelemetryManager& _telemetry; Output::OutputIBUS _ibus; Sensor::GpsSensor _gps; diff --git a/lib/Espfc/src/Vtx.h b/lib/Espfc/src/Vtx.h new file mode 100644 index 00000000..11ede483 --- /dev/null +++ b/lib/Espfc/src/Vtx.h @@ -0,0 +1,23 @@ +#pragma once + +namespace Espfc::Connect { + +enum VtxDeviceType { + VTXDEV_UNSUPPORTED = 0, // reserved for MSP + VTXDEV_RTC6705 = 1, + // 2 reserved + VTXDEV_SMARTAUDIO = 3, + VTXDEV_TRAMP = 4, + VTXDEV_MSP = 5, + VTXDEV_UNKNOWN = 0xFF, +}; + +enum State { + INACTIVE, + INIT, + SET_POWER, + SET_CHANNEL, + IDLE, +}; + +} \ No newline at end of file From cbdc07cf41c22e2ee75eed38add377d553a3244f Mon Sep 17 00:00:00 2001 From: adam Date: Tue, 25 Mar 2025 01:39:14 +0100 Subject: [PATCH 08/11] interval, constructor --- lib/Espfc/src/Connect/VtxSmartAudio.cpp | 2 +- lib/Espfc/src/Connect/VtxTramp.cpp | 5 ++--- lib/Espfc/src/Connect/VtxTramp.hpp | 15 ++++++++++----- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/lib/Espfc/src/Connect/VtxSmartAudio.cpp b/lib/Espfc/src/Connect/VtxSmartAudio.cpp index 9ee62af6..117bbbe4 100644 --- a/lib/Espfc/src/Connect/VtxSmartAudio.cpp +++ b/lib/Espfc/src/Connect/VtxSmartAudio.cpp @@ -8,7 +8,7 @@ namespace Espfc::Connect { int VtxSmartAudio::begin(Device::SerialDevice * serial) { _serial = serial; - _timer.setRate(300); + _timer.setInterval(300); _state = State::INIT; return 1; diff --git a/lib/Espfc/src/Connect/VtxTramp.cpp b/lib/Espfc/src/Connect/VtxTramp.cpp index 1d1befb2..46833ac2 100644 --- a/lib/Espfc/src/Connect/VtxTramp.cpp +++ b/lib/Espfc/src/Connect/VtxTramp.cpp @@ -8,7 +8,7 @@ namespace Espfc::Connect { int VtxTramp::begin(Device::SerialDevice * serial) { _serial = serial; - _timer.setRate(300); + _timer.setInterval(300); _state = State::INIT; return 1; @@ -16,9 +16,8 @@ int VtxTramp::begin(Device::SerialDevice * serial) int VtxTramp::initTramp() { - // Send initialization command TrampCommand initCmd; - initCmd.command = 'r'; // 'r' for reset/init + initCmd.command = 'r'; initCmd.crc = Utils::crc8_dvb_s2(0, reinterpret_cast(&initCmd), sizeof(initCmd) - 2); _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); _serial->flush(); diff --git a/lib/Espfc/src/Connect/VtxTramp.hpp b/lib/Espfc/src/Connect/VtxTramp.hpp index 2785097a..6347b879 100644 --- a/lib/Espfc/src/Connect/VtxTramp.hpp +++ b/lib/Espfc/src/Connect/VtxTramp.hpp @@ -29,11 +29,16 @@ class VtxTramp struct TrampCommand { - uint8_t header[2] = {0x0F, 0x00}; // Header bytes - uint8_t command; // Command identifier - uint8_t payload[12] = {0}; // Data payload - uint8_t crc; // CRC byte - uint8_t terminator = 0x00; // Terminator byte + uint8_t header[2]; + uint8_t command; + uint8_t payload[12]; + uint8_t crc; + uint8_t terminator; + + TrampCommand() + : header{0x0F, 0x00}, command(0), payload{0}, crc(0), terminator(0x00) + { + } }; From fdd101da1b54baa280ecb6883a6e14b91af25457 Mon Sep 17 00:00:00 2001 From: adam Date: Wed, 26 Mar 2025 16:06:09 +0100 Subject: [PATCH 09/11] rework protocol --- lib/Espfc/src/Connect/VtxSmartAudio.hpp | 1 - lib/Espfc/src/Connect/VtxTramp.cpp | 44 ++++++++----------------- lib/Espfc/src/Connect/VtxTramp.hpp | 30 ++++++++--------- 3 files changed, 29 insertions(+), 46 deletions(-) diff --git a/lib/Espfc/src/Connect/VtxSmartAudio.hpp b/lib/Espfc/src/Connect/VtxSmartAudio.hpp index b919d659..54d6f14a 100644 --- a/lib/Espfc/src/Connect/VtxSmartAudio.hpp +++ b/lib/Espfc/src/Connect/VtxSmartAudio.hpp @@ -26,5 +26,4 @@ class VtxSmartAudio Utils::Timer _timer; }; - } diff --git a/lib/Espfc/src/Connect/VtxTramp.cpp b/lib/Espfc/src/Connect/VtxTramp.cpp index 46833ac2..e11f7b25 100644 --- a/lib/Espfc/src/Connect/VtxTramp.cpp +++ b/lib/Espfc/src/Connect/VtxTramp.cpp @@ -1,8 +1,6 @@ #include "VtxTramp.hpp" #include "Utils/Crc.hpp" -static const uint8_t dummyByte[] = { 0x00 }; - namespace Espfc::Connect { int VtxTramp::begin(Device::SerialDevice * serial) @@ -14,23 +12,12 @@ int VtxTramp::begin(Device::SerialDevice * serial) return 1; } -int VtxTramp::initTramp() -{ - TrampCommand initCmd; - initCmd.command = 'r'; - initCmd.crc = Utils::crc8_dvb_s2(0, reinterpret_cast(&initCmd), sizeof(initCmd) - 2); - _serial->write(reinterpret_cast(&initCmd), sizeof(initCmd)); - _serial->flush(); - return 1; -} - int VtxTramp::update() { if (!_timer.check()) return 1; switch (_state) { case State::INIT: - initTramp(); _state = State::SET_CHANNEL; _model.state.vtx.active = true; break; @@ -57,31 +44,28 @@ int VtxTramp::update() return 1; } -int VtxTramp::setChannel() +int VtxTramp::trampSendCommand(uint8_t cmd, uint16_t param) { - uint8_t vtxCommand[6]; + uint8_t vtxCommand[15]; vtxCommand[0] = 0x0F; - vtxCommand[1] = 0x55; - vtxCommand[2] = 0x00; - vtxCommand[3] = 0x00; - vtxCommand[4] = (_model.config.vtx.band - 1) * 8 + (_model.config.vtx.channel - 1); - vtxCommand[5] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 5); - _serial->write(vtxCommand, 6); + vtxCommand[1] = cmd; + vtxCommand[2] = param & 0xff; + vtxCommand[3] = (param >> 8) & 0xff; + vtxCommand[14] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 13); + _serial->write(vtxCommand, 16); _serial->flush(); return 1; } +int VtxTramp::setChannel() +{ + trampSendCommand('F', TrampFreqTable[_model.config.vtx.band -1][_model.config.vtx.channel -1]); + return 1; +} + int VtxTramp::setPower() { - uint8_t vtxCommand[6]; - vtxCommand[0] = 0x0F; - vtxCommand[1] = 0x56; - vtxCommand[2] = 0x00; - vtxCommand[3] = 0x00; - vtxCommand[4] = (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power : 0; - vtxCommand[5] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 5); - _serial->write(vtxCommand, 6); - _serial->flush(); + trampSendCommand('P', (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? TrampPowerTable[_model.config.vtx.power -1] : 0); return 1; } diff --git a/lib/Espfc/src/Connect/VtxTramp.hpp b/lib/Espfc/src/Connect/VtxTramp.hpp index 6347b879..997628a4 100644 --- a/lib/Espfc/src/Connect/VtxTramp.hpp +++ b/lib/Espfc/src/Connect/VtxTramp.hpp @@ -17,7 +17,22 @@ class VtxTramp int update(); int setChannel(); int setPower(); + int trampSendCommand(uint8_t cmd, uint16_t param); Connect::VtxDeviceType type = Connect::VTXDEV_TRAMP; + const uint16_t TrampFreqTable[8][8] { + {5865, 5845, 5825, 5805, 5785, 5765, 5745, 0}, + {5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, + {5740, 5760, 5780, 5800, 5820, 5840, 5860, 0}, + {0, 0, 5732, 5769, 5806, 5843, 0, 0}, + {5732, 5765, 5828, 5840, 5866, 5740, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + }; + const uint16_t TrampPowerTable[5] { + 25, 100, 200, 400, 600 + }; + private: Device::SerialDevice* _serial; @@ -27,19 +42,4 @@ class VtxTramp Utils::Timer _timer; }; - -struct TrampCommand { - uint8_t header[2]; - uint8_t command; - uint8_t payload[12]; - uint8_t crc; - uint8_t terminator; - - TrampCommand() - : header{0x0F, 0x00}, command(0), payload{0}, crc(0), terminator(0x00) - { - } -}; - - } From a1c2ec75b616cad398fc913dd60e0a5f4aef1c97 Mon Sep 17 00:00:00 2001 From: adam Date: Wed, 26 Mar 2025 16:06:37 +0100 Subject: [PATCH 10/11] update readme --- README.md | 7 ++ docs/vtx_table_irc_tramp.json | 105 +++++++++++++++++++++++++++++ docs/vtx_table_tbs_smartaudio.json | 104 ++++++++++++++++++++++++++++ 3 files changed, 216 insertions(+) create mode 100644 docs/vtx_table_irc_tramp.json create mode 100644 docs/vtx_table_tbs_smartaudio.json diff --git a/README.md b/README.md index 16951883..48f866dd 100644 --- a/README.md +++ b/README.md @@ -134,6 +134,13 @@ Here are more details about [how to setup](/docs/setup.md). | QMC5883 | Yes | Yes | Yes | | AK8963 | Yes | Yes | Yes | +## Supported VTX devices + +| Device | ESP8266 | ESP32 | RP2040 | +|------------------:|--------:|------:|-------:| +| [TBS SmartAudio](/docs/vtx_table_tbs_smartaudio.json) | Yes | Yes | Yes | +| [IRC Tramp](/docs/vtx_table_irc_tramp.json) | Yes | Yes | Yes | + ## Issues You can report issues using Github [tracker](https://github.com/rtlopez/esp-fc/issues) You can also join to our [Discord Channel](https://discord.gg/jhyPPM5UEH) diff --git a/docs/vtx_table_irc_tramp.json b/docs/vtx_table_irc_tramp.json new file mode 100644 index 00000000..c4248942 --- /dev/null +++ b/docs/vtx_table_irc_tramp.json @@ -0,0 +1,105 @@ +{ + "description": "ESPFC VTX Config file for IRC Tramp", + "version": "1.0", + "vtx_table": { + "bands_list": [ + { + "name": "BOSCAM_A", + "letter": "A", + "is_factory_band": false, + "frequencies": [ + 5865, + 5845, + 5825, + 5805, + 5785, + 5765, + 5745, + 0 + ] + }, + { + "name": "BOSCAM_B", + "letter": "B", + "is_factory_band": false, + "frequencies": [ + 5733, + 5752, + 5771, + 5790, + 5809, + 5828, + 5847, + 5866 + ] + }, + { + "name": "FATSHARK", + "letter": "F", + "is_factory_band": false, + "frequencies": [ + 5740, + 5760, + 5780, + 5800, + 5820, + 5840, + 5860, + 0 + ] + }, + { + "name": "RACEBAND", + "letter": "R", + "is_factory_band": false, + "frequencies": [ + 0, + 0, + 5732, + 5769, + 5806, + 5843, + 0, + 0 + ] + }, + { + "name": "IMD6", + "letter": "I", + "is_factory_band": false, + "frequencies": [ + 5732, + 5765, + 5828, + 5840, + 5866, + 5740, + 0, + 0 + ] + } + ], + "powerlevels_list": [ + { + "value": 25, + "label": "25 " + }, + { + "value": 100, + "label": "100 " + }, + { + "value": 200, + "label": "200 " + }, + { + "value": 400, + "label": "400 " + }, + { + "value": 600, + "label": "600 " + } + ] + } +} diff --git a/docs/vtx_table_tbs_smartaudio.json b/docs/vtx_table_tbs_smartaudio.json new file mode 100644 index 00000000..41858f72 --- /dev/null +++ b/docs/vtx_table_tbs_smartaudio.json @@ -0,0 +1,104 @@ +{ + "description": "ESPFC VTX Config file for SmartAudio 2.0", + "version": "1.0", + "vtx_table": { + "bands_list": [ + { + "name": "BOSCAM_A", + "letter": "A", + "is_factory_band": true, + "frequencies": [ + 5865, + 5845, + 5825, + 5805, + 5785, + 5765, + 5745, + 0 + ] + }, + { + "name": "BOSCAM_B", + "letter": "B", + "is_factory_band": true, + "frequencies": [ + 5733, + 5752, + 5771, + 5790, + 5809, + 5828, + 5847, + 5866 + ] + }, + { + "name": "UNKNOWN", + "letter": "U", + "is_factory_band": true, + "frequencies": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ] + }, + { + "name": "FATSHARK", + "letter": "F", + "is_factory_band": true, + "frequencies": [ + 5740, + 5760, + 5780, + 5800, + 5820, + 5840, + 5860, + 0 + ] + }, + { + "name": "RACEBAND", + "letter": "R", + "is_factory_band": true, + "frequencies": [ + 0, + 0, + 5732, + 5769, + 5806, + 5843, + 0, + 0 + ] + }, + { + "name": "IMD6", + "letter": "I", + "is_factory_band": false, + "frequencies": [ + 5732, + 5765, + 5828, + 5840, + 5866, + 5740, + 0, + 0 + ] + } + ], + "powerlevels_list": [ + { + "value": 0, + "label": "25 " + } + ] + } +} From 7385841fa616c6022dd191a461f1f9a8f446ad5b Mon Sep 17 00:00:00 2001 From: adam Date: Fri, 31 Oct 2025 15:03:17 +0100 Subject: [PATCH 11/11] vtx channels, and pitmode init --- docs/vtx_table_irc_tramp.json | 100 +++++++++++++++++++++++------ lib/Espfc/src/Connect/VtxTramp.cpp | 32 ++++++--- lib/Espfc/src/Connect/VtxTramp.hpp | 25 ++++---- lib/Espfc/src/ModelConfig.h | 1 + lib/Espfc/src/Utils/Crc.cpp | 14 ++-- lib/Espfc/src/Utils/Crc.hpp | 2 +- 6 files changed, 125 insertions(+), 49 deletions(-) diff --git a/docs/vtx_table_irc_tramp.json b/docs/vtx_table_irc_tramp.json index c4248942..bbf69165 100644 --- a/docs/vtx_table_irc_tramp.json +++ b/docs/vtx_table_irc_tramp.json @@ -15,7 +15,7 @@ 5785, 5765, 5745, - 0 + 5725 ] }, { @@ -33,6 +33,21 @@ 5866 ] }, + { + "name": "BOSCAM_E", + "letter": "E", + "is_factory_band": false, + "frequencies": [ + 5705, + 5685, + 5665, + 5645, + 5885, + 5905, + 5925, + 5945 + ] + }, { "name": "FATSHARK", "letter": "F", @@ -45,7 +60,7 @@ 5820, 5840, 5860, - 0 + 5880 ] }, { @@ -53,29 +68,74 @@ "letter": "R", "is_factory_band": false, "frequencies": [ - 0, - 0, + 5658, + 5695, 5732, 5769, 5806, 5843, - 0, - 0 + 5880, + 5917 + ] + }, + { + "name": "U", + "letter": "U", + "is_factory_band": false, + "frequencies": [ + 5325, + 5348, + 5366, + 5384, + 5402, + 5420, + 5438, + 5456 ] }, { - "name": "IMD6", - "letter": "I", + "name": "O", + "letter": "O", "is_factory_band": false, "frequencies": [ - 5732, - 5765, - 5828, - 5840, - 5866, - 5740, - 0, - 0 + 5474, + 5492, + 5510, + 5528, + 5546, + 5564, + 5582, + 5600 + ] + }, + { + "name": "L", + "letter": "L", + "is_factory_band": false, + "frequencies": [ + 5333, + 5373, + 5413, + 5453, + 5493, + 5533, + 5573, + 5613 + ] + }, + { + "name": "H", + "letter": "H", + "is_factory_band": false, + "frequencies": [ + 5653, + 5693, + 5733, + 5773, + 5813, + 5853, + 5893, + 5933 ] } ], @@ -93,12 +153,12 @@ "label": "200 " }, { - "value": 400, - "label": "400 " + "value": 350, + "label": "350 " }, { - "value": 600, - "label": "600 " + "value": 400, + "label": "400 " } ] } diff --git a/lib/Espfc/src/Connect/VtxTramp.cpp b/lib/Espfc/src/Connect/VtxTramp.cpp index e11f7b25..81acb1ac 100644 --- a/lib/Espfc/src/Connect/VtxTramp.cpp +++ b/lib/Espfc/src/Connect/VtxTramp.cpp @@ -46,15 +46,19 @@ int VtxTramp::update() int VtxTramp::trampSendCommand(uint8_t cmd, uint16_t param) { - uint8_t vtxCommand[15]; - vtxCommand[0] = 0x0F; - vtxCommand[1] = cmd; - vtxCommand[2] = param & 0xff; - vtxCommand[3] = (param >> 8) & 0xff; - vtxCommand[14] = Utils::crc8_dvb_s2(0, reinterpret_cast(&vtxCommand), 13); - _serial->write(vtxCommand, 16); - _serial->flush(); - return 1; + uint8_t vtxCommand[16] = {0}; // fully zeroed like Betaflight + vtxCommand[0] = 0x0F; // sync + vtxCommand[1] = cmd; // command + vtxCommand[2] = param & 0xFF; + vtxCommand[3] = (param >> 8) & 0xFF; + + // bytes 4..13 already zeroed + vtxCommand[14] = Utils::crc8_dvb_s2(0, vtxCommand, 14); // CRC over bytes 0–13 + vtxCommand[15] = 0x00; // end byte + + _serial->write(vtxCommand, 16); + _serial->flush(); + return 1; } int VtxTramp::setChannel() @@ -65,7 +69,15 @@ int VtxTramp::setChannel() int VtxTramp::setPower() { - trampSendCommand('P', (!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? TrampPowerTable[_model.config.vtx.power -1] : 0); + uint16_t power = (!_model.config.vtx.lowPowerDisarm/* || _model.isModeActive(MODE_ARMED)*/) ? + TrampPowerTable[_model.config.vtx.power - 1] : 0; + trampSendCommand('P', power); + return 1; +} + +int VtxTramp::setPitMode() +{ + trampSendCommand('I', _model.config.vtx.pitMode ? 0 : 1); return 1; } diff --git a/lib/Espfc/src/Connect/VtxTramp.hpp b/lib/Espfc/src/Connect/VtxTramp.hpp index 997628a4..2bbbc2f1 100644 --- a/lib/Espfc/src/Connect/VtxTramp.hpp +++ b/lib/Espfc/src/Connect/VtxTramp.hpp @@ -17,23 +17,24 @@ class VtxTramp int update(); int setChannel(); int setPower(); + int setPitMode(); int trampSendCommand(uint8_t cmd, uint16_t param); Connect::VtxDeviceType type = Connect::VTXDEV_TRAMP; - const uint16_t TrampFreqTable[8][8] { - {5865, 5845, 5825, 5805, 5785, 5765, 5745, 0}, - {5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, - {5740, 5760, 5780, 5800, 5820, 5840, 5860, 0}, - {0, 0, 5732, 5769, 5806, 5843, 0, 0}, - {5732, 5765, 5828, 5840, 5866, 5740, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0}, + const uint16_t TrampFreqTable[9][8] = { + {5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, // BOSCAM_A + {5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, // BOSCAM_B + {5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945}, // BOSCAM_E + {5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880}, // FATSHARK + {5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917}, // RACEBAND + {5325, 5348, 5366, 5384, 5402, 5420, 5438, 5456}, // U + {5474, 5492, 5510, 5528, 5546, 5564, 5582, 5600}, // O + {5333, 5373, 5413, 5453, 5493, 5533, 5573, 5613}, // L + {5653, 5693, 5733, 5773, 5813, 5853, 5893, 5933} // H }; - const uint16_t TrampPowerTable[5] { - 25, 100, 200, 400, 600 + const uint16_t TrampPowerTable[5] = { + 25, 100, 200, 400, 600 }; - private: Device::SerialDevice* _serial; Model& _model; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 311816e0..a58daa3e 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -657,6 +657,7 @@ struct VtxConfig uint8_t band = 0x1; uint8_t power = 0; uint8_t lowPowerDisarm = 0; + uint8_t pitMode = 0; }; struct GpsConfig diff --git a/lib/Espfc/src/Utils/Crc.cpp b/lib/Espfc/src/Utils/Crc.cpp index d3f734ed..19feb38d 100644 --- a/lib/Espfc/src/Utils/Crc.cpp +++ b/lib/Espfc/src/Utils/Crc.cpp @@ -22,13 +22,15 @@ uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t a) return crc; } -uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len) +uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t *data, int length) { - while (len-- > 0) - { - crc = crc8_dvb_s2(crc, *data++); - } - return crc; + while (length--) { + crc ^= *data++; + for (int i = 0; i < 8; i++) { + crc = (crc & 0x80) ? ((crc << 1) ^ 0xD5) : (crc << 1); + } + } + return crc; } uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t a) diff --git a/lib/Espfc/src/Utils/Crc.hpp b/lib/Espfc/src/Utils/Crc.hpp index a6abdb5e..9fe95277 100644 --- a/lib/Espfc/src/Utils/Crc.hpp +++ b/lib/Espfc/src/Utils/Crc.hpp @@ -8,7 +8,7 @@ namespace Espfc { namespace Utils { uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t a); -uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len); +uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t *data, int length); uint8_t crc8_xor(uint8_t checksum, const uint8_t a); uint8_t crc8_xor(uint8_t checksum, const uint8_t *data, size_t len);