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