Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

added MSP_VTX_CONFIG & MSP_SET_VTX_CONFIG

Browse source
This commit is contained in:
Raphael Coeffic 2017-01-17 01:06:20 +01:00
parent bdbd7b1a8a
commit ebce3d9847
1 changed files with 67 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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