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 #include "AP_RCProtocol_DSM.h" +#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) +#include "AP_RCProtocol_SRXL2.h" +#endif #include 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 #include #include #include +#include #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 +#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: