Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: configure VTX when a SRXL2 VTX packet is received

Browse Source 
support DSM VTX control
This commit is contained in:
Andy Piper 2020-07-08 21:56:16 +01:00 committed by Peter Barker
parent 111a4a735b
commit b697a44aed
3 changed files with 80 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -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;
}

56
libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
View File

@ -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);
}

3
libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h
View File

@ -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: