AP_RCProtocol: configure VTX when a SRXL2 VTX packet is received

support DSM VTX control

libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp

@@ -18,7 +18,12 @@
  * See https://www.spektrumrc.com/ProdInfo/Files/Remote%20Receiver%20Interfacing%20Rev%20A.pdf for official
  * Spektrum documentation on the format.
  */
+
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 #include "AP_RCProtocol_DSM.h"
+#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
+#include "AP_RCProtocol_SRXL2.h"
+#endif
 #include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
@@ -40,6 +45,16 @@ extern const AP_HAL::HAL& hal;
 #define SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
 #define SPEKTRUM_VTX_CONTROL_FRAME      0xe000e000
 
+#define SPEKTRUM_VTX_BAND_MASK          0x00e00000
+#define SPEKTRUM_VTX_CHANNEL_MASK       0x000f0000
+#define SPEKTRUM_VTX_PIT_MODE_MASK      0x00000010
+#define SPEKTRUM_VTX_POWER_MASK         0x00000007
+
+#define SPEKTRUM_VTX_BAND_SHIFT         21
+#define SPEKTRUM_VTX_CHANNEL_SHIFT      16
+#define SPEKTRUM_VTX_PIT_MODE_SHIFT     4
+#define SPEKTRUM_VTX_POWER_SHIFT        0
+
 void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
 {
     uint8_t b;
@@ -68,7 +83,13 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_us, const uint8_t dsm_fra
     if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME 
         && (dsm_frame[2] & 0x80) == 0)  {
         dsm_frame_data_size = AP_DSM_FRAME_SIZE - 4;
-        // someday do something with the data
+#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
+        AP_RCProtocol_SRXL2::configure_vtx(
+            (vtxControl & SPEKTRUM_VTX_BAND_MASK)     >> SPEKTRUM_VTX_BAND_SHIFT,
+            (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK)  >> SPEKTRUM_VTX_CHANNEL_SHIFT,
+            (vtxControl & SPEKTRUM_VTX_POWER_MASK)    >> SPEKTRUM_VTX_POWER_SHIFT,
+            (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT);
+#endif
     } else {
         dsm_frame_data_size = AP_DSM_FRAME_SIZE;
     }

libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp

@@ -18,12 +18,12 @@
  */
 
 #include "AP_RCProtocol.h"
-#include "AP_RCProtocol_SRXL.h"
 #include "AP_RCProtocol_SRXL2.h"
 #include <AP_Math/AP_Math.h>
 #include <AP_RCTelemetry/AP_Spektrum_Telem.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_HAL/utility/sparse-endian.h>
+#include <AP_RCTelemetry/AP_VideoTX.h>
 
 #include "spm_srxl.h"
 
@@ -241,6 +241,59 @@ void AP_RCProtocol_SRXL2::send_on_uart(uint8_t* pBuffer, uint8_t length)
     }
 }
 
+// configure the video transmitter, the input values are Spektrum-oriented
+void AP_RCProtocol_SRXL2::configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode)
+{
+    AP_VideoTX& vtx = AP::vtx();
+    // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
+    // map to TBS band A, B, E, Race, Airwave, LoRace
+    switch (band) {
+    case VTX_BAND_FATSHARK:
+        vtx.set_configured_band(AP_VideoTX::VideoBand::FATSHARK);
+        break;
+    case VTX_BAND_RACEBAND:
+        vtx.set_configured_band(AP_VideoTX::VideoBand::RACEBAND);
+        break;
+    case VTX_BAND_E_BAND:
+        vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_E);
+        break;
+    case VTX_BAND_B_BAND:
+        vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_B);
+        break;
+    case VTX_BAND_A_BAND:
+        vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_A);
+        break;
+    default:
+        break;
+    }
+    // VTX Channel (0-7)
+    vtx.set_configured_channel(channel);
+    if (pitmode) {
+        vtx.set_configured_options(vtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    } else {
+        vtx.set_configured_options(vtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    }
+
+    switch (power) {
+    case VTX_POWER_1MW_14MW:
+    case VTX_POWER_15MW_25MW:
+        vtx.set_configured_power_mw(25);
+        break;
+    case VTX_POWER_26MW_99MW:
+    case VTX_POWER_100MW_299MW:
+        vtx.set_configured_power_mw(100);
+        break;
+    case VTX_POWER_300MW_600MW:
+        vtx.set_configured_power_mw(400);
+        break;
+    case VTX_POWER_601_PLUS:
+        vtx.set_configured_power_mw(800);
+        break;
+    default:
+        break;
+    }
+}
+
 // send data to the uart
 void AP_RCProtocol_SRXL2::_send_on_uart(uint8_t* pBuffer, uint8_t length)
 {
@@ -343,4 +396,5 @@ bool srxlOnBind(SrxlFullID device, SrxlBindData info)
 // User-provided callback routine to handle reception of a VTX control packet.
 void srxlOnVtx(SrxlVtxData* pVtxData)
 {
+    AP_RCProtocol_SRXL2::configure_vtx(pVtxData->band, pVtxData->channel, pVtxData->power, pVtxData->pit);
 }

libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h

@@ -17,6 +17,7 @@
 
 #include "AP_RCProtocol.h"
 #include <AP_Math/AP_Math.h>
+#include "AP_RCProtocol_SRXL.h"
 #include "SoftSerial.h"
 
 #define SRXL2_MAX_CHANNELS 32U           /* Maximum number of channels from srxl2 datastream  */
@@ -39,6 +40,8 @@ public:
     static void capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi);
     static void send_on_uart(uint8_t* pBuffer, uint8_t length);
     static void change_baud_rate(uint32_t baudrate);
+    // configure the VTX from Spektrum data
+    static void configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode);
 
 private: