diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index f877b2fc..73fa91c2 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(3 /* SMARTAUDIO */); // vtx type unknown + 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 diff --git a/lib/Espfc/src/Connect/Vtx.cpp b/lib/Espfc/src/Connect/Vtx.cpp index c972f5ca..c1604dfa 100644 --- a/lib/Espfc/src/Connect/Vtx.cpp +++ b/lib/Espfc/src/Connect/Vtx.cpp @@ -57,11 +57,19 @@ int Vtx::update() _model.state.vtx.active = true; break; case State::SET_POWER: - setPower(); + if (type == VTXDEV_TRAMP) { + setTrampPower(); + } else { + setPower(); + } _state = State::IDLE; break; case State::SET_CHANNEL: - setChannel(); + if (type == VTXDEV_TRAMP) { + setTrampChannel(); + } else { + setChannel(); + } _state = State::SET_POWER; break; case State::IDLE: @@ -101,4 +109,24 @@ int Vtx::setPower() return 1; } +int Vtx::setTrampChannel() +{ + uint8_t vtxCommand[6] = { 0x0F, 0x55, 0xAA, 0x03, (uint8_t)((_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::setTrampPower() +{ + uint8_t vtxCommand[6] = { 0x0F, 0x55, 0xAA, 0x02, (uint8_t)((!_model.config.vtx.lowPowerDisarm || _model.isModeActive(MODE_ARMED)) ? _model.config.vtx.power - 1 : 0) }; + vtxCommand[5] = crc8(vtxCommand, 5); + _serial->write(vtxCommand, 6); + _serial->flush(); + + return 1; +} + } diff --git a/lib/Espfc/src/Connect/Vtx.hpp b/lib/Espfc/src/Connect/Vtx.hpp index a6ae2956..5e23584b 100644 --- a/lib/Espfc/src/Connect/Vtx.hpp +++ b/lib/Espfc/src/Connect/Vtx.hpp @@ -33,6 +33,8 @@ class Vtx int update(); int setChannel(); int setPower(); + int setTrampChannel(); + int setTrampPower(); Connect::VtxDeviceType type; private: diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index f4805d64..2a69d153 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) { @@ -122,7 +129,7 @@ int SerialManager::begin() { _ibus.begin(port); } - if(spc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO) + if(spc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO || spc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) { _vtx.begin(port); } @@ -161,7 +168,7 @@ int FAST_CODE_ATTR SerialManager::update() { _ibus.update(); } - if(sc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO) + if(sc.functionMask & SERIAL_FUNCTION_VTX_SMARTAUDIO || sc.functionMask & SERIAL_FUNCTION_VTX_TRAMP) { _vtx.update(); }