rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2146 from raphaelcoeffic/vtx-msp 

Vtx MSP requests (rebased / squashed)
This commit is contained in:
Michael Keller 2017-01-26 09:23:38 +13:00 committed by GitHub
parent af8443c867 ebce3d9847
commit bce0415218
4 changed files with 107 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/drivers/vtx_common.c
View File

@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs)
vtxDevType_e vtxCommonGetDeviceType(void)
{
if (!vtxDevice)
if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
return VTXDEV_UNKNOWN;
return vtxDevice->devtype;
return vtxDevice->vTable->getDeviceType();
}
// band and chan are 1 origin
@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan)
if (!vtxDevice)
return;
if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan))
return;
if (vtxDevice->vTable->setBandChan)
vtxDevice->vTable->setBandChan(band, chan);
}
@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index)
if (!vtxDevice)
return;
if (index > vtxDevice->numPower)
return;
if (vtxDevice->vTable->setPowerByIndex)
vtxDevice->vTable->setPowerByIndex(index);
}

7
src/main/drivers/vtx_common.h
View File

@ -18,9 +18,12 @@
/* Created by jflyper */
typedef enum {
VTXDEV_UNKNOWN = 0,
VTXDEV_UNSUPPORTED = 0, // reserved for MSP
// 1 reserved
// 2 reserved
VTXDEV_SMARTAUDIO = 3,
VTXDEV_TRAMP = 4,
VTXDEV_UNKNOWN = 0xFF,
} vtxDevType_e;
struct vtxVTable_s;
@ -28,8 +31,6 @@ struct vtxVTable_s;
typedef struct vtxDevice_s {
const struct vtxVTable_s *vTable;
vtxDevType_e devtype; // 3.1 only; eventually goes away
uint8_t numBand;
uint8_t numChan;
uint8_t numPower;

75
src/main/fc/fc_msp.c
View File

@ -47,6 +47,7 @@
#include "drivers/vtx_soft_spi_rtc6705.h"
#include "drivers/pwm_output.h"
#include "drivers/serial_escserial.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "fc/fc_core.h"
@ -1187,6 +1188,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
}
break;
#if defined(VTX_COMMON)
case MSP_VTX_CONFIG:
{
uint8_t deviceType = vtxCommonGetDeviceType();
if (deviceType != VTXDEV_UNKNOWN) {
uint8_t band=0, channel=0;
vtxCommonGetBandChan(&band,&channel);
uint8_t powerIdx=0; // debug
vtxCommonGetPowerIndex(&powerIdx);
uint8_t pitmode=0;
vtxCommonGetPitmode(&pitmode);
sbufWriteU8(dst, deviceType);
sbufWriteU8(dst, band);
sbufWriteU8(dst, channel);
sbufWriteU8(dst, powerIdx);
sbufWriteU8(dst, pitmode);
// future extensions here...
}
else {
sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
}
}
#endif
break;
default:
return false;
}
@ -1634,15 +1664,44 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
#endif
#ifdef USE_RTC6705
#if defined(USE_RTC6705) || defined(VTX_COMMON)
case MSP_SET_VTX_CONFIG:
;
uint16_t tmp = sbufReadU16(src);
if (tmp < 40)
masterConfig.vtx_channel = tmp;
if (current_vtx_channel != masterConfig.vtx_channel) {
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
{
uint16_t tmp = sbufReadU16(src);
#if defined(USE_RTC6705)
if (tmp < 40)
masterConfig.vtx_channel = tmp;
if (current_vtx_channel != masterConfig.vtx_channel) {
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
}
#else
if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
uint8_t band = (tmp / 8) + 1;
uint8_t channel = (tmp % 8) + 1;
uint8_t current_band=0, current_channel=0;
vtxCommonGetBandChan(&current_band,&current_channel);
if ((current_band != band) || (current_channel != channel))
vtxCommonSetBandChan(band,channel);
if (sbufBytesRemaining(src) < 2)
break;
uint8_t power = sbufReadU8(src);
uint8_t current_power = 0;
vtxCommonGetPowerIndex(&current_power);
if (current_power != power)
vtxCommonSetPowerByIndex(power);
uint8_t pitmode = sbufReadU8(src);
uint8_t current_pitmode = 0;
vtxCommonGetPitmode(&current_pitmode);
if (current_pitmode != pitmode)
vtxCommonSetPitmode(pitmode);
}
#endif
}
break;
#endif

57
src/main/io/vtx_tramp.c
View File

@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = {
};
static const char * const trampPowerNames[] = {
"25 ", "100", "200", "400", "600"
"---", "25 ", "100", "200", "400", "600"
};
#endif
#if defined(VTX_COMMON)
static vtxVTable_t trampVTable; // Forward
static vtxDevice_t vtxTramp = {
.vTable = &trampVTable,
.vTable = NULL,
.numBand = 5,
.numChan = 8,
.numPower = sizeof(trampPowerTable),
@ -309,26 +308,6 @@ void trampQueryS(void)
trampQuery('s');
}
bool trampInit()
{
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
if (portConfig) {
trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
}
if (!trampSerialPort) {
return false;
}
#if defined(VTX_COMMON)
vtxTramp.vTable = &trampVTable;
vtxCommonRegisterDevice(&vtxTramp);
#endif
return true;
}
void vtxTrampProcess(uint32_t currentTimeUs)
{
static uint32_t lastQueryTimeUs = 0;
@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }
static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
static uint8_t trampCmsPower = 0;
static uint8_t trampCmsPower = 1;
static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL };
static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL };
static void trampCmsUpdateFreqRef(void)
{
@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
UNUSED(self);
trampSetBandChan(trampCmsBand, trampCmsChan);
trampSetRFPower(trampPowerTable[trampCmsPower]);
trampSetRFPower(trampPowerTable[trampCmsPower-1]);
// If it fails, the user should retry later
trampCommitChanges();
@ -559,7 +538,7 @@ static void trampCmsInitSettings()
if (trampCurConfigPower > 0) {
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
if (trampCurConfigPower <= trampPowerTable[i]) {
trampCmsPower = i;
trampCmsPower = i + 1;
break;
}
}
@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan)
void vtxTrampSetPowerByIndex(uint8_t index)
{
if (index) {
trampSetRFPower(trampPowerTable[index]);
trampSetRFPower(trampPowerTable[index - 1]);
trampCommitChanges();
}
}
@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
if (trampCurConfigPower > 0) {
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
if (trampCurConfigPower <= trampPowerTable[i]) {
*pIndex = i;
*pIndex = i + 1;
break;
}
}
@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = {
#endif
bool trampInit()
{
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
if (portConfig) {
trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
}
if (!trampSerialPort) {
return false;
}
#if defined(VTX_COMMON)
vtxTramp.vTable = &trampVTable;
vtxCommonRegisterDevice(&vtxTramp);
#endif
return true;
}
#endif // VTX_TRAMP