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 extern bool ICACHE_RAM_ATTR IsArmed();
  11 extern CRSF crsf;
  12 extern MSP msp;
  13
  14 static enum VtxSendState_e
  15 {
  16   VTXSS_UNKNOWN,   // Status of the remote side is unknown, so we should send immediately if connected
  17   VTXSS_MODIFIED,  // Config is editied, should always be sent regardless of connect state
  18   VTXSS_SENDING1, VTXSS_SENDING2, VTXSS_SENDING3,  VTXSS_SENDINGDONE, // Send the config 3x
  19   VTXSS_CONFIRMED  // Status of remote side is consistent with our config
  20 } VtxSendState;
  21
  22 void VtxTriggerSend()
  23 {
  24     VtxSendState = VTXSS_MODIFIED;
  25     devicesTriggerEvent();
  26 }
  27
  28 static void eepromWriteToMSPOut()
  29 {
  30     mspPacket_t packet;
  31     packet.reset();
  32     packet.function = MSP_EEPROM_WRITE;
  33     packet.addByte(0);
  34     packet.addByte(0);
  35     packet.addByte(0);
  36     packet.addByte(0);
  37
  38     crsf.AddMspMessage(&packet);
  39 }
  40
  41 static void VtxConfigToMSPOut()
  42 {
  43     DBGLN("Sending VtxConfig");
  44     uint8_t vtxIdx = (config.GetVtxBand()-1) * 8 + config.GetVtxChannel();
  45
  46     mspPacket_t packet;
  47     packet.reset();
  48     packet.makeCommand();
  49     packet.function = MSP_SET_VTX_CONFIG;
  50     packet.addByte(vtxIdx);
  51     packet.addByte(0);
  52     packet.addByte(config.GetVtxPower());
  53     packet.addByte(config.GetVtxPitmode());
  54
  55     crsf.AddMspMessage(&packet);
  56     msp.sendPacket(&packet, &Serial); // send to tx-backpack as MSP
  57 }
  58
  59 static int event()
  60 {
  61     if (VtxSendState == VTXSS_MODIFIED ||
  62         (VtxSendState == VTXSS_UNKNOWN && connectionState == connected))
  63     {
  64         VtxSendState = VTXSS_SENDING1;
  65         return 1000;
  66     }
  67
  68     if (connectionState == disconnected)
  69         VtxSendState = VTXSS_UNKNOWN;
  70
  71     return DURATION_NEVER;
  72 }
  73
  74 static int timeout()
  75 {
  76     // 0 = off in the lua Band field
  77     // Do not send while armed
  78     if (config.GetVtxBand() == 0 || IsArmed())
  79     {
  80         VtxSendState = VTXSS_CONFIRMED;
  81         return DURATION_NEVER;
  82     }
  83
  84     VtxConfigToMSPOut();
  85
  86     VtxSendState = (VtxSendState_e)((int)VtxSendState + 1);
  87     if (VtxSendState < VTXSS_SENDINGDONE)
  88         return 500; // repeat send in 500ms
  89
  90     if (connectionState == connected)
  91     {
  92         // Connected while sending, assume the MSP got to the RX
  93         VtxSendState = VTXSS_CONFIRMED;
  94         eepromWriteToMSPOut();
  95     }
  96     else
  97     {
  98         VtxSendState = VTXSS_UNKNOWN;
  99         // Never received a connection, clear the queue which now
 100         // has multiple VTX config packets in it
 101         crsf.ResetMspQueue();
 102     }
 103
 104     return DURATION_NEVER;
 105 }
 106
 107 device_t VTX_device = {
 108     .initialize = NULL,
 109     .start = NULL,
 110     .event = event,
 111     .timeout = timeout
 112 };