From b697a44aed74c11ece4e83ae16643c73ad197c4b Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 8 Jul 2020 21:56:16 +0100
Subject: [PATCH] AP_RCProtocol: configure VTX when a SRXL2 VTX packet is
received
support DSM VTX control
---
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp | 23 +++++++-
.../AP_RCProtocol/AP_RCProtocol_SRXL2.cpp | 56 ++++++++++++++++++-
libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h | 3 +
3 files changed, 80 insertions(+), 2 deletions(-)
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
index eab104ed6e..a1729ce6ba 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
+++ b/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;
}
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
index 4cd8f60cce..bbd1628a03 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
+++ b/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);
}
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h
index 42bbbc0946..37ab0527ee 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h
+++ b/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: