Initial vtx support (IRC Tramp)

lib/Espfc/src/Connect/MspProcessor.cpp

@@ -1403,29 +1403,29 @@ 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.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
+        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;
+          // const uint8_t newBand = (freq / 8) + 1;
+          // const uint8_t newChannel = (freq % 8) + 1;
         }
 
         if (m.remain() >= 2) {
@@ -1453,7 +1453,6 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       }
       break;
 
-
     case MSP_SET_ARMING_DISABLED:
       {
         const uint8_t cmd = m.readU8();

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<uint8_t*>(&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<uint8_t*>(&vtxCommand), 5);
+  _serial->write(dummyByte, 1);
+  _serial->write(vtxCommand, 6);
+  _serial->flush();
+  return 1;
+}
+
+}

lib/Espfc/src/Connect/Vtx.hpp → 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;
 };
 
+
 }

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<uint8_t*>(&initCmd), sizeof(initCmd) - 2);
+  _serial->write(reinterpret_cast<uint8_t*>(&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<uint8_t*>(&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<uint8_t*>(&vtxCommand), 5);
+  _serial->write(vtxCommand, 6);
+  _serial->flush();
+  return 1;
+}
+
+}