Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge pull request #3401 from iNavFlight/agh_vtx_settings

Browse source 
Port new VTX API and settings from Betaflight
This commit is contained in:
Alberto García Hierro 2018-06-22 09:20:24 +01:00 • committed by GitHub
commit 5ed9236f75
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
37 changed files with 1422 additions and 594 deletions
Download patch file Download diff file Expand all files Collapse all files

91
src/main/fc/fc_msp.c
View file

@ -76,6 +76,8 @@
#include "io/osd.h"
#include "io/serial.h"
#include "io/serial_4way.h"
#include "io/vtx.h"
#include "io/vtx_string.h"
#include "msp/msp.h"
#include "msp/msp_protocol.h"
@ -1270,30 +1272,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
break;
#if defined(VTX_COMMON)
#if defined(USE_VTX_COMMON)
case MSP_VTX_CONFIG:
{
uint8_t deviceType = vtxCommonGetDeviceType();
if (deviceType != VTXDEV_UNKNOWN) {
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
uint8_t band=0, channel=0;
vtxCommonGetBandAndChannel(&band,&channel);
uint8_t deviceType = 0;
vtxCommonGetDeviceType(vtxDevice);
uint8_t powerIdx=0; // debug
vtxCommonGetPowerIndex(&powerIdx);
uint8_t pitmode=0;
vtxCommonGetPitMode(&pitmode);
// Return band, channel and power from vtxSettingsConfig_t
// since the VTX might be configured but temporarily offline.
uint8_t pitmode = 0;
vtxCommonGetPitMode(vtxDevice, &pitmode);
sbufWriteU8(dst, deviceType);
sbufWriteU8(dst, band);
sbufWriteU8(dst, channel);
sbufWriteU8(dst, powerIdx);
sbufWriteU8(dst, vtxSettingsConfig()->band);
sbufWriteU8(dst, vtxSettingsConfig()->channel);
sbufWriteU8(dst, vtxSettingsConfig()->power);
sbufWriteU8(dst, pitmode);
// Betaflight doesn't send these fields
sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0);
sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
// future extensions here...
}
else {
sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured
}
}
break;
@ -2144,36 +2149,44 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
#endif // USE_OSD
#if defined(VTX_COMMON)
#if defined(USE_VTX_COMMON)
case MSP_SET_VTX_CONFIG:
if (dataSize >= 4) {
tmp_u16 = sbufReadU16(src);
const uint8_t band = (tmp_u16 / 8) + 1;
const uint8_t channel = (tmp_u16 % 8) + 1;
if (dataSize >= 2) {
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
uint16_t newFrequency = sbufReadU16(src);
if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
const uint8_t newBand = (newFrequency / 8) + 1;
const uint8_t newChannel = (newFrequency % 8) + 1;
vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
} else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { //value is frequency in MHz. Ignore it if it's invalid
vtxSettingsConfigMutable()->band = 0;
vtxSettingsConfigMutable()->channel = 0;
vtxSettingsConfigMutable()->freq = newFrequency;
}
if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
uint8_t current_band=0, current_channel=0;
vtxCommonGetBandAndChannel(&current_band,&current_channel);
if ((current_band != band) || (current_channel != channel))
vtxCommonSetBandAndChannel(band,channel);
if (sbufBytesRemaining(src) > 1) {
vtxSettingsConfigMutable()->power = sbufReadU8(src);
// Delegate pitmode to vtx directly
const uint8_t newPitmode = sbufReadU8(src);
uint8_t currentPitmode = 0;
vtxCommonGetPitMode(vtxDevice, &currentPitmode);
if (currentPitmode != newPitmode) {
vtxCommonSetPitMode(vtxDevice, newPitmode);
}
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);
if (sbufBytesRemaining(src) > 0) {
vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
}
}
}
}
} else
} else {
return MSP_RESULT_ERROR;
}
break;
#endif