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
  34     crsf.AddMspMessage(&packet);
  35 }
  36
  37 static void VtxConfigToMSPOut()
  38 {
  39     DBGLN("Sending VtxConfig");
  40     uint8_t vtxIdx = (config.GetVtxBand()-1) * 8 + config.GetVtxChannel();
  41
  42     mspPacket_t packet;
  43     packet.reset();
  44     packet.makeCommand();
  45     packet.function = MSP_SET_VTX_CONFIG;
  46     packet.addByte(vtxIdx);
  47     packet.addByte(0);
  48     if (config.GetVtxPower()) {
  49         packet.addByte(config.GetVtxPower());
  50         packet.addByte(config.GetVtxPitmode());
  51     }
  52
  53     crsf.AddMspMessage(&packet);
  54     msp.sendPacket(&packet, &Serial); // send to tx-backpack as MSP
  55 }
  56
  57 static int event()
  58 {
  59     if (VtxSendState == VTXSS_MODIFIED ||
  60         (VtxSendState == VTXSS_UNKNOWN && connectionState == connected))
  61     {
  62         VtxSendState = VTXSS_SENDING1;
  63         return 1000;
  64     }
  65
  66     if (connectionState == disconnected)
  67         VtxSendState = VTXSS_UNKNOWN;
  68
  69     return DURATION_NEVER;
  70 }
  71
  72 static int timeout()
  73 {
  74     // 0 = off in the lua Band field
  75     // Do not send while armed
  76     if (config.GetVtxBand() == 0 || IsArmed())
  77     {
  78         VtxSendState = VTXSS_CONFIRMED;
  79         return DURATION_NEVER;
  80     }
  81
  82     VtxConfigToMSPOut();
  83
  84     VtxSendState = (VtxSendState_e)((int)VtxSendState + 1);
  85     if (VtxSendState < VTXSS_SENDINGDONE)
  86         return 500; // repeat send in 500ms
  87
  88     if (connectionState == connected)
  89     {
  90         // Connected while sending, assume the MSP got to the RX
  91         VtxSendState = VTXSS_CONFIRMED;
  92         eepromWriteToMSPOut();
  93     }
  94     else
  95     {
  96         VtxSendState = VTXSS_UNKNOWN;
  97         // Never received a connection, clear the queue which now
  98         // has multiple VTX config packets in it
  99         crsf.ResetMspQueue();
 100     }
 101
 102     return DURATION_NEVER;
 103 }
 104
 105 device_t VTX_device = {
 106     .initialize = NULL,
 107     .start = NULL,
 108     .event = event,
 109     .timeout = timeout
 110 };