src/lib/VTX/devVTX.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3 #include "device.h"
   4
   5 #include "config.h"
   6 #include "CRSF.h"
   7 #include "msp.h"
   8 #include "logging.h"
   9
  10 #include "devButton.h"
  11 #include "handset.h"
  12
  13 #define PITMODE_OFF     0
  14 #define PITMODE_ON      1
  15
  16 // Delay after disconnect to preserve the VTXSS_CONFIRMED status
  17 // Needs to be long enough to reconnect, but short enough to
  18 // reset between the user switching equipment
  19 #define VTX_DISCONNECT_DEBOUNCE_MS (10 * 1000)
  20
  21 extern Stream *TxBackpack;
  22 static uint8_t pitmodeAuxState = 0;
  23 static bool sendEepromWrite = true;
  24
  25 static enum VtxSendState_e
  26 {
  27   VTXSS_UNKNOWN,   // Status of the remote side is unknown, so we should send immediately if connected
  28   VTXSS_MODIFIED,  // Config is editied, should always be sent regardless of connect state
  29   VTXSS_SENDING1, VTXSS_SENDING2, VTXSS_SENDING3,  VTXSS_SENDINGDONE, // Send the config 3x
  30   VTXSS_CONFIRMED  // Status of remote side is consistent with our config
  31 } VtxSendState;
  32
  33 void VtxTriggerSend()
  34 {
  35     VtxSendState = VTXSS_MODIFIED;
  36     devicesTriggerEvent();
  37 }
  38
  39 void VtxPitmodeSwitchUpdate()
  40 {
  41     if (config.GetVtxPitmode() <= PITMODE_ON)
  42     {
  43         pitmodeAuxState = config.GetVtxPitmode();
  44         return;
  45     }
  46
  47     uint8_t auxInverted = config.GetVtxPitmode() % 2;
  48     uint8_t auxNumber = (config.GetVtxPitmode() / 2) + 3;
  49     uint8_t newPitmodeAuxState = CRSF_to_BIT(ChannelData[auxNumber]) ^ auxInverted;
  50
  51     if (pitmodeAuxState != newPitmodeAuxState)
  52     {
  53         pitmodeAuxState = newPitmodeAuxState;
  54         sendEepromWrite = false;
  55         VtxTriggerSend();
  56     }
  57 }
  58
  59 static void eepromWriteToMSPOut()
  60 {
  61     mspPacket_t packet;
  62     packet.reset();
  63     packet.function = MSP_EEPROM_WRITE;
  64
  65     CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER);
  66 }
  67
  68 static void VtxConfigToMSPOut()
  69 {
  70     DBGLN("Sending VtxConfig");
  71     uint8_t vtxIdx = (config.GetVtxBand()-1) * 8 + config.GetVtxChannel();
  72
  73     mspPacket_t packet;
  74     packet.reset();
  75     packet.makeCommand();
  76     packet.function = MSP_SET_VTX_CONFIG;
  77     packet.addByte(vtxIdx);     // band/channel or frequency low byte
  78     packet.addByte(0);          // frequency high byte, if frequency mode
  79     if (config.GetVtxPower())
  80     {
  81         packet.addByte(config.GetVtxPower());
  82         packet.addByte(pitmodeAuxState);
  83     }
  84
  85     CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER);
  86
  87     if (!handset->IsArmed()) // Do not send while armed.  There is no need to change the video frequency while armed.  It can also cause VRx modules to flash up their OSD menu e.g. Rapidfire.
  88     {
  89         MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
  90     }
  91 }
  92
  93 static void initialize()
  94 {
  95     registerButtonFunction(ACTION_SEND_VTX, VtxTriggerSend);
  96 }
  97
  98 static int event()
  99 {
 100     if (VtxSendState == VTXSS_MODIFIED ||
 101         (VtxSendState == VTXSS_UNKNOWN && connectionState == connected))
 102     {
 103         VtxSendState = VTXSS_SENDING1;
 104         return 1000;
 105     }
 106
 107     if (connectionState == disconnected)
 108     {
 109         // If the VtxSend has completed, wait before going back to VTXSS_UNKNOWN
 110         // to ignore a temporary disconnect after saving EEPROM
 111         if (VtxSendState == VTXSS_CONFIRMED)
 112         {
 113             VtxSendState = VTXSS_CONFIRMED;
 114             return VTX_DISCONNECT_DEBOUNCE_MS;
 115         }
 116         VtxSendState = VTXSS_UNKNOWN;
 117     }
 118     else if (VtxSendState == VTXSS_CONFIRMED && connectionState == connected)
 119     {
 120         return DURATION_NEVER;
 121     }
 122
 123     return DURATION_IGNORE;
 124 }
 125
 126 static int timeout()
 127 {
 128     // 0 = off in the lua Band field
 129     if (config.GetVtxBand() == 0)
 130     {
 131         VtxSendState = VTXSS_CONFIRMED;
 132         return DURATION_NEVER;
 133     }
 134
 135     // Can only get here in VTXSS_CONFIRMED state if still disconnected
 136     if (VtxSendState == VTXSS_CONFIRMED)
 137     {
 138         VtxSendState = VTXSS_UNKNOWN;
 139         return DURATION_NEVER;
 140     }
 141
 142     VtxConfigToMSPOut();
 143
 144     VtxSendState = (VtxSendState_e)((int)VtxSendState + 1);
 145     if (VtxSendState < VTXSS_SENDINGDONE)
 146         return 500; // repeat send in 500ms
 147
 148     if (connectionState == connected)
 149     {
 150         // Connected while sending, assume the MSP got to the RX
 151         VtxSendState = VTXSS_CONFIRMED;
 152         if (sendEepromWrite)
 153             eepromWriteToMSPOut();
 154         sendEepromWrite = true;
 155     }
 156     else
 157     {
 158         VtxSendState = VTXSS_UNKNOWN;
 159         // Never received a connection, clear the queue which now
 160         // has multiple VTX config packets in it
 161         CRSF::ResetMspQueue();
 162     }
 163
 164     return DURATION_NEVER;
 165 }
 166
 167 device_t VTX_device = {
 168     .initialize = initialize,
 169     .start = NULL,
 170     .event = event,
 171     .timeout = timeout
 172 };