diff --git a/make/source.mk b/make/source.mk index ce50b0500d..52a9fd2c6c 100644 --- a/make/source.mk +++ b/make/source.mk @@ -137,6 +137,7 @@ COMMON_SRC = \ cms/cms_menu_osd.c \ cms/cms_menu_vtx_smartaudio.c \ cms/cms_menu_vtx_tramp.c \ + cms/cms_menu_vtx_ffpv.c \ common/colorconversion.c \ common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ @@ -189,6 +190,7 @@ COMMON_SRC = \ io/vtx_string.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ + io/vtx_ffpv24g.c \ io/vtx_control.c COMMON_DEVICE_SRC = \ diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 32336c4788..712691d898 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -53,6 +53,7 @@ #include "cms/cms_menu_vtx_smartaudio.h" #include "cms/cms_menu_vtx_tramp.h" +#include "cms/cms_menu_vtx_ffpv.h" // Info @@ -119,6 +120,9 @@ static const OSD_Entry menuFeaturesEntries[] = #if defined(USE_VTX_TRAMP) OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp), #endif +#if defined(USE_VTX_FFPV) + OSD_SUBMENU_ENTRY("VTX FFPV", &cmsx_menuVtxFFPV), +#endif #endif // VTX_CONTROL #ifdef USE_LED_STRIP OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index ac0aae5e7f..48177f7666 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -166,9 +166,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS), OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR), OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES), -#ifdef USE_VTX_COMMON OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL), -#endif // VTX OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW), OSD_ELEMENT_ENTRY("POWER", OSD_POWER), OSD_ELEMENT_ENTRY("USED MAH", OSD_MAH_DRAWN), @@ -252,7 +250,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_END_ENTRY, }; -#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) +#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) // All CMS OSD elements should be enabled in this case. The menu has 3 extra // elements (label, back and end), but there's an OSD element that we intentionally // don't show here (OSD_DEBUG). diff --git a/src/main/cms/cms_menu_vtx_ffpv.c b/src/main/cms/cms_menu_vtx_ffpv.c new file mode 100644 index 0000000000..f0c17d335d --- /dev/null +++ b/src/main/cms/cms_menu_vtx_ffpv.c @@ -0,0 +1,216 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_CMS) && defined(USE_VTX_FFPV) + +#include "common/printf.h" +#include "common/utils.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" + +#include "drivers/vtx_common.h" + +#include "fc/config.h" + +#include "io/vtx_string.h" +#include "io/vtx_ffpv24g.h" +#include "io/vtx.h" + +static bool ffpvCmsDrawStatusString(char *buf, unsigned bufsize) +{ + const char *defaultString = "- -- ---- ---"; +// m bc ffff ppp +// 01234567890123 + + if (bufsize < strlen(defaultString) + 1) { + return false; + } + + strcpy(buf, defaultString); + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_FFPV || !vtxCommonDeviceIsReady(vtxDevice)) { + return true; + } + + buf[0] = '*'; + buf[1] = ' '; + buf[2] = ffpvBandLetters[ffpvGetRuntimeState()->band]; + buf[3] = ffpvChannelNames[ffpvGetRuntimeState()->channel][0]; + buf[4] = ' '; + + tfp_sprintf(&buf[5], "%4d", ffpvGetRuntimeState()->frequency); + tfp_sprintf(&buf[9], " %3d", ffpvGetRuntimeState()->powerMilliwatt); + + return true; +} + +uint8_t ffpvCmsBand = 1; +uint8_t ffpvCmsChan = 1; +uint16_t ffpvCmsFreqRef; +static uint8_t ffpvCmsPower = 1; + +static const OSD_TAB_t ffpvCmsEntBand = { &ffpvCmsBand, VTX_FFPV_BAND_COUNT, ffpvBandNames }; +static const OSD_TAB_t ffpvCmsEntChan = { &ffpvCmsChan, VTX_FFPV_CHANNEL_COUNT, ffpvChannelNames }; +static const OSD_TAB_t ffpvCmsEntPower = { &ffpvCmsPower, VTX_FFPV_POWER_COUNT, ffpvPowerNames }; + +static void ffpvCmsUpdateFreqRef(void) +{ + if (ffpvCmsBand > 0 && ffpvCmsChan > 0) { + ffpvCmsFreqRef = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1]; + } +} + +static long ffpvCmsConfigBand(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsBand == 0) { + // Bounce back + ffpvCmsBand = 1; + } + else { + ffpvCmsUpdateFreqRef(); + } + + return 0; +} + +static long ffpvCmsConfigChan(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsChan == 0) { + // Bounce back + ffpvCmsChan = 1; + } + else { + ffpvCmsUpdateFreqRef(); + } + + return 0; +} + +static long ffpvCmsConfigPower(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsPower == 0) { + // Bounce back + ffpvCmsPower = 1; + } + + return 0; +} + +static long ffpvCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + // call driver directly + ffpvSetBandAndChannel(ffpvCmsBand, ffpvCmsChan); + ffpvSetRFPowerByIndex(ffpvCmsPower); + + // update'vtx_' settings + vtxSettingsConfigMutable()->band = ffpvCmsBand; + vtxSettingsConfigMutable()->channel = ffpvCmsChan; + vtxSettingsConfigMutable()->power = ffpvCmsPower; + vtxSettingsConfigMutable()->freq = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1]; + + saveConfigAndNotify(); + + return MENU_CHAIN_BACK; +} + +static void ffpvCmsInitSettings(void) +{ + ffpvCmsBand = ffpvGetRuntimeState()->band; + ffpvCmsChan = ffpvGetRuntimeState()->channel; + ffpvCmsPower = ffpvGetRuntimeState()->powerIndex; + + ffpvCmsUpdateFreqRef(); +} + +static long ffpvCmsOnEnter(const OSD_Entry *from) +{ + UNUSED(from); + + ffpvCmsInitSettings(); + return 0; +} + +static const OSD_Entry ffpvCmsMenuCommenceEntries[] = +{ + OSD_LABEL_ENTRY("CONFIRM"), + OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +static const CMS_Menu ffpvCmsMenuCommence = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXTRC", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = ffpvCmsMenuCommenceEntries, +}; + +static const OSD_Entry ffpvMenuEntries[] = +{ + OSD_LABEL_ENTRY("- TRAMP -"), + + OSD_LABEL_FUNC_DYN_ENTRY("", ffpvCmsDrawStatusString), + OSD_TAB_CALLBACK_ENTRY("BAND", ffpvCmsConfigBand, &ffpvCmsEntBand), + OSD_TAB_CALLBACK_ENTRY("CHAN", ffpvCmsConfigChan, &ffpvCmsEntChan), + OSD_UINT16_RO_ENTRY("(FREQ)", &ffpvCmsFreqRef), + OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower), + OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +const CMS_Menu cmsx_menuVtxFFPV = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXTR", + .GUARD_type = OME_MENU, +#endif + .onEnter = ffpvCmsOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = ffpvMenuEntries, +}; +#endif diff --git a/src/main/cms/cms_menu_vtx_ffpv.h b/src/main/cms/cms_menu_vtx_ffpv.h new file mode 100644 index 0000000000..6faf7cb59a --- /dev/null +++ b/src/main/cms/cms_menu_vtx_ffpv.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "cms/cms.h" +#include "cms/cms_types.h" + +extern const CMS_Menu cmsx_menuVtxFFPV; diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index d916f03bc8..34b2cf27e8 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -25,8 +25,6 @@ #include "platform.h" #include "build/debug.h" -#if defined(USE_VTX_COMMON) - #include "vtx_common.h" static vtxDevice_t *commonVtxDevice = NULL; @@ -152,4 +150,3 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t } return false; } -#endif diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 2ef2a84983..7e7ee7c9c2 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -72,6 +72,7 @@ typedef enum { // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, + VTXDEV_FFPV = 5, VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2a51d74910..ba2a3caee1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -107,6 +107,7 @@ #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" +#include "io/vtx_ffpv24g.h" #include "io/piniobox.h" #include "msp/msp_serial.h" @@ -637,13 +638,10 @@ void init(void) pitotStartCalibration(); #endif -#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) vtxControlInit(); - -#if defined(USE_VTX_COMMON) vtxCommonInit(); vtxInit(); -#endif #ifdef USE_VTX_SMARTAUDIO vtxSmartAudioInit(); @@ -653,7 +651,11 @@ void init(void) vtxTrampInit(); #endif -#endif // USE_VTX_COMMON && USE_VTX_CONTROL +#ifdef USE_VTX_FFPV + vtxFuriousFPVInit(); +#endif + +#endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 71e3325da8..6a95777400 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1280,7 +1280,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; -#if defined(USE_VTX_COMMON) case MSP_VTX_CONFIG: { vtxDevice_t *vtxDevice = vtxCommonDevice(); @@ -1309,7 +1308,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; -#endif case MSP_NAME: { @@ -2158,7 +2156,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif // USE_OSD -#if defined(USE_VTX_COMMON) case MSP_SET_VTX_CONFIG: if (dataSize >= 2) { vtxDevice_t *vtxDevice = vtxCommonDevice(); @@ -2197,7 +2194,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; } break; -#endif #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 9b0e0e0d06..1a8f9069c1 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -531,11 +531,11 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) [TASK_VTXCTRL] = { .taskName = "VTXCTRL", .taskFunc = vtxUpdate, - .desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec + .desiredPeriod = TASK_PERIOD_HZ(50), // 50Hz @20msec .staticPriority = TASK_PRIORITY_IDLE, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 97ccfced81..a67aefca3c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1697,7 +1697,7 @@ groups: - name: PG_VTX_CONFIG type: vtxConfig_t headers: ["io/vtx_control.h"] - condition: USE_VTX_CONTROL && USE_VTX_COMMON + condition: USE_VTX_CONTROL members: - name: vtx_halfduplex field: halfDuplex @@ -1706,7 +1706,6 @@ groups: - name: PG_VTX_SETTINGS_CONFIG type: vtxSettingsConfig_t headers: ["drivers/vtx_common.h", "io/vtx.h"] - condition: USE_VTX_COMMON members: - name: vtx_band field: band diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9eca2a6491..4eb6eeb950 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1588,7 +1588,6 @@ static bool osdDrawSingleElement(uint8_t item) break; } -#if defined(VTX) || defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: #if defined(VTX) // FIXME: This doesn't actually work. It's for boards with @@ -1616,7 +1615,6 @@ static bool osdDrawSingleElement(uint8_t item) } #endif break; -#endif // VTX || VTX_COMMON case OSD_CROSSHAIRS: osdCrosshairsBounds(&elemPosX, &elemPosY, NULL); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index ee1ed5a8dd..565543c6f9 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -48,6 +48,7 @@ typedef enum { FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_DEBUG_TRACE = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 + FUNCTION_VTX_FFPV = (1 << 17), // 131072 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 1dc0e0d50a..16274209a5 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -23,8 +23,6 @@ #include "platform.h" -#if defined(USE_VTX_COMMON) - #include "common/maths.h" #include "common/time.h" @@ -275,5 +273,3 @@ void vtxUpdate(timeUs_t currentTimeUs) } } } - -#endif diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 6daebbe8e7..4853a6fd06 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -37,7 +37,7 @@ #include "io/vtx_control.h" -#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) +#if defined(USE_VTX_CONTROL) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index e39e447a9f..3b07168891 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -33,6 +33,15 @@ typedef struct vtxConfig_s { uint8_t halfDuplex; } vtxConfig_t; +typedef struct vtxRunState_s { + int pitMode; + int band; + int channel; + int frequency; + int powerIndex; + int powerMilliwatt; +} vtxRunState_t; + PG_DECLARE(vtxConfig_t, vtxConfig); void vtxControlInit(void); diff --git a/src/main/io/vtx_ffpv24g.c b/src/main/io/vtx_ffpv24g.c new file mode 100644 index 0000000000..32521f89bf --- /dev/null +++ b/src/main/io/vtx_ffpv24g.c @@ -0,0 +1,531 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#include +#include +#include +#include + +#if defined(USE_VTX_FFPV) && defined(USE_VTX_CONTROL) + +#include "build/debug.h" + +#include "drivers/time.h" +#include "drivers/vtx_common.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "scheduler/protothreads.h" + +//#include "cms/cms_menu_vtx_ffpv24g.h" + +#include "io/vtx.h" +#include "io/vtx_ffpv24g.h" +#include "io/vtx_control.h" +#include "io/vtx_string.h" +#include "io/serial.h" + + +#define VTX_FFPV_CMD_TIMEOUT_MS 250 +#define VTX_FFPV_HEARTBEAT_MS 1000 + +#define VTX_FFPV_MIN_BAND (1) +#define VTX_FFPV_MAX_BAND (VTX_FFPV_MIN_BAND + VTX_FFPV_BAND_COUNT - 1) +#define VTX_FFPV_MIN_CHANNEL (1) +#define VTX_FFPV_MAX_CHANNEL (VTX_FFPV_MIN_CHANNEL + VTX_FFPV_CHANNEL_COUNT -1) + +#define VTX_UPDATE_REQ_NONE 0x00 +#define VTX_UPDATE_REQ_FREQUENCY 0x01 +#define VTX_UPDATE_REQ_POWER 0x02 + +typedef struct __attribute__((__packed__)) ffpvPacket_s { + uint8_t header; + uint8_t cmd; + uint8_t data[12]; + uint8_t checksum; + uint8_t footer; +} ffpvPacket_t; + +typedef struct { + bool ready; + int protoTimeouts; + unsigned updateReqMask; + + // VTX capabilities + struct { + unsigned freqMin; + unsigned freqMax; + unsigned powerMin; + unsigned powerMax; + } capabilities; + + // Requested VTX state + struct { + bool setByFrequency; + int band; + int channel; + unsigned freq; + unsigned power; + unsigned powerIndex; + } request; + + // Actual VTX state + struct { + unsigned freq; + unsigned power; + } state; + + // Comms flags and state + ffpvPacket_t sendPkt; + ffpvPacket_t recvPkt; + unsigned recvPtr; + bool pktReceived; +} vtxProtoState_t; + +/*****************************************************************************/ +const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = { + "--------", + "FFPV 2.4 A", + "FFPV 2.4 B", +}; + +const char * ffpvBandLetters = "-AB"; + +const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT] = +{ + { 2410, 2430, 2450, 2470, 2370, 2390, 2490, 2510 }, // FFPV 2.4 A + { 2414, 2432, 2450, 2468, 2411, 2433, 2453, 2473 }, // FFPV 2.4 A +}; + +const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1] = { + "---", "25 ", "200", "500", "800" +}; + +const unsigned ffpvPowerTable[VTX_FFPV_POWER_COUNT] = { + 25, 200, 500, 800 +}; + + +/*******************************************************************************/ +static serialPort_t * vtxSerialPort = NULL; +static vtxProtoState_t vtxState; + +static uint8_t vtxCalcChecksum(ffpvPacket_t * pkt) +{ + uint8_t sum = pkt->cmd; + for (int i = 0; i < 12; i++) { + sum += pkt->data[i]; + } + return sum; +} + +static bool vtxProtoRecv(void) +{ + // Return success instantly if packet is already awaiting processing + if (vtxState.pktReceived) { + return true; + } + + uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt; + while (serialRxBytesWaiting(vtxSerialPort) && !vtxState.pktReceived) { + const uint8_t c = serialRead(vtxSerialPort); + + if (vtxState.recvPtr == 0) { + // Wait for sync byte + if (c == 0x0F) { + bufPtr[vtxState.recvPtr++] = c; + } + } + else { + // Sync byte ok - wait for full packet + if (vtxState.recvPtr < sizeof(vtxState.recvPkt)) { + bufPtr[vtxState.recvPtr++] = c; + } + + // Received full packet - do some processing + if (vtxState.recvPtr == sizeof(vtxState.recvPkt)) { + // Full packet received - validate packet, make sure it's the one we expect + const bool pktValid = (vtxState.recvPkt.header == 0x0F && vtxState.recvPkt.cmd == vtxState.sendPkt.cmd && vtxState.recvPkt.footer == 0x00 && vtxState.recvPkt.checksum == vtxCalcChecksum(&vtxState.recvPkt)); + if (!pktValid) { + // Reset the receiver state - keep waiting + vtxState.pktReceived = false; + vtxState.recvPtr = 0; + } + // Make sure it's not the echo one (half-duplex serial might receive it's own data) + else if (memcmp(&vtxState.recvPkt.data, &vtxState.sendPkt.data, sizeof(vtxState.recvPkt.data)) == 0) { + vtxState.pktReceived = false; + vtxState.recvPtr = 0; + } + // Valid receive packet + else { + vtxState.pktReceived = true; + return true; + } + } + } + } + + return false; +} + +static void vtxProtoSend(uint8_t cmd, const uint8_t * data) +{ + // Craft and send FPV packet + vtxState.sendPkt.header = 0x0F; + vtxState.sendPkt.cmd = cmd; + + if (data) { + memcpy(vtxState.sendPkt.data, data, sizeof(vtxState.sendPkt.data)); + } + else { + memset(vtxState.sendPkt.data, 0, sizeof(vtxState.sendPkt.data)); + } + + vtxState.sendPkt.checksum = vtxCalcChecksum(&vtxState.sendPkt); + vtxState.sendPkt.footer = 0x00; + + // Send data + serialWriteBuf(vtxSerialPort, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt)); + + // Reset cmd response state + vtxState.pktReceived = false; + vtxState.recvPtr = 0; +} + +static void vtxProtoSend_SetFreqency(unsigned freq) +{ + uint8_t data[12] = {0}; + data[0] = freq & 0xFF; + data[1] = (freq >> 8) & 0xFF; + vtxProtoSend(0x46, data); +} + +static void vtxProtoSend_SetPower(unsigned power) +{ + uint8_t data[12] = {0}; + data[0] = power & 0xFF; + data[1] = (power >> 8) & 0xFF; + vtxProtoSend(0x50, data); +} + +STATIC_PROTOTHREAD(impl_VtxProtocolThread) +{ + ptBegin(impl_VtxProtocolThread); + + // 0: Detect VTX. Dwell here infinitely until we get a valid response from VTX + vtxState.ready = false; + while(!vtxState.ready) { + // Send capabilities request and wait + vtxProtoSend(0x72, NULL); + ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS); + + // Check if we got a valid response + if (vtxState.pktReceived) { + vtxState.capabilities.freqMin = vtxState.recvPkt.data[0] | (vtxState.recvPkt.data[1] << 8); + vtxState.capabilities.freqMax = vtxState.recvPkt.data[2] | (vtxState.recvPkt.data[3] << 8); + vtxState.capabilities.powerMin = 0; + vtxState.capabilities.powerMax = vtxState.recvPkt.data[4] | (vtxState.recvPkt.data[5] << 8); + vtxState.ready = true; + } + } + + // 1 : Periodically poll VTX for current channel and power, send updates + vtxState.protoTimeouts = 0; + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + + while(vtxState.ready) { + // Wait for request for update or time to check liveness + ptWaitTimeout(vtxState.updateReqMask != VTX_UPDATE_REQ_NONE, VTX_FFPV_HEARTBEAT_MS); + + if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) { + if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) { + vtxProtoSend_SetFreqency(vtxState.request.freq); + vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY; + ptDelayMs(VTX_FFPV_CMD_TIMEOUT_MS); + } + else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) { + vtxProtoSend_SetPower(vtxState.request.power); + vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER; + } + else { + // Unsupported request - reset + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + } + } + else { + // Periodic check for VTX liveness + vtxProtoSend(0x76, NULL); + ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS); + + if (vtxState.pktReceived) { + // Got a valid state from VTX + vtxState.state.freq = (uint16_t)vtxState.recvPkt.data[0] | ((uint16_t)vtxState.recvPkt.data[1] << 8); + vtxState.state.power = (uint16_t)vtxState.recvPkt.data[2] | ((uint16_t)vtxState.recvPkt.data[3] << 8); + vtxState.protoTimeouts = 0; + + // Check if VTX state matches VTX request + if (vtxState.state.freq != vtxState.request.freq) { + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; + } + + if (vtxState.state.power != vtxState.request.power) { + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; + } + } + else { + vtxState.protoTimeouts++; + } + } + + // Sanity check. If we got more than 3 protocol erros + if (vtxState.protoTimeouts >= 3) { + // Reset ready flag - thread will terminate and restart + vtxState.ready = false; + } + } + + ptEnd(0); +} + + +static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + // Glue function betwen VTX VTable and actual driver protothread + UNUSED(vtxDevice); + UNUSED(currentTimeUs); + + impl_VtxProtocolThread(); + + // If thread stopped - vtx comms failed - restart thread and re-init VTX comms + if (ptIsStopped(ptGetHandle(impl_VtxProtocolThread))) { + ptRestart(ptGetHandle(impl_VtxProtocolThread)); + } +} + +static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice) +{ + UNUSED(vtxDevice); + return VTXDEV_FFPV; +} + +static bool impl_IsReady(const vtxDevice_t *vtxDevice) +{ + return vtxDevice != NULL && vtxSerialPort != NULL && vtxState.ready; +} + +static bool impl_DevSetFreq(uint16_t freq) +{ + if (!vtxState.ready || freq < vtxState.capabilities.freqMin || freq > vtxState.capabilities.freqMax) { + return false; + } + + vtxState.request.freq = freq; + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; + + return true; +} + +static void impl_SetFreq(vtxDevice_t * vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + + if (impl_DevSetFreq(freq)) { + // Keep track that we set frequency directly + vtxState.request.setByFrequency = true; + } +} + +void ffpvSetBandAndChannel(uint8_t band, uint8_t channel) +{ + // Validate band and channel + if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) { + return; + } + + if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) { + // Keep track of band/channel data + vtxState.request.setByFrequency = false; + vtxState.request.band = band; + vtxState.request.channel = channel; + } +} + + +static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel) +{ + UNUSED(vtxDevice); + ffpvSetBandAndChannel(band, channel); +} + +void ffpvSetRFPowerByIndex(uint16_t index) +{ + // Validate index + if (index < 1 || index > VTX_FFPV_POWER_COUNT) { + return; + } + + const unsigned power = ffpvPowerTable[index - 1]; + if (!vtxState.ready || power < vtxState.capabilities.powerMin || power > vtxState.capabilities.powerMax) { + return; + } + + vtxState.request.power = power; + vtxState.request.powerIndex = index; + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; +} + +static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index) +{ + UNUSED(vtxDevice); + ffpvSetRFPowerByIndex(index); +} + +static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + // TODO: Not implemented + UNUSED(vtxDevice); + UNUSED(onoff); +} + +static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + // if in user-freq mode then report band as zero + *pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band; + *pChannel = vtxState.request.channel; + return true; +} + +static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + *pIndex = vtxState.request.powerIndex; + + return true; +} + +static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + // TODO: Not inplemented + *pOnOff = 0; + return true; +} + +static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + *pFreq = vtxState.request.freq; + return true; +} + +vtxRunState_t * ffpvGetRuntimeState(void) +{ + static vtxRunState_t state; + + if (vtxState.ready) { + state.pitMode = 0; + state.band = vtxState.request.band; + state.channel = vtxState.request.channel; + state.frequency = vtxState.request.freq; + state.powerIndex = vtxState.request.powerIndex; + state.powerMilliwatt = vtxState.request.power; + } + else { + state.pitMode = 0; + state.band = 1; + state.channel = 1; + state.frequency = ffpvFrequencyTable[0][0]; + state.powerIndex = 1; + state.powerMilliwatt = 25; + } + return &state; +} + +/*****************************************************************************/ +static const vtxVTable_t impl_vtxVTable = { + .process = impl_Process, + .getDeviceType = impl_GetDeviceType, + .isReady = impl_IsReady, + .setBandAndChannel = impl_SetBandAndChannel, + .setPowerByIndex = impl_SetPowerByIndex, + .setPitMode = impl_SetPitMode, + .setFrequency = impl_SetFreq, + .getBandAndChannel = impl_GetBandAndChannel, + .getPowerIndex = impl_GetPowerIndex, + .getPitMode = impl_GetPitMode, + .getFrequency = impl_GetFreq, +}; + +static vtxDevice_t impl_vtxDevice = { + .vTable = &impl_vtxVTable, + .capability.bandCount = VTX_FFPV_BAND_COUNT, + .capability.channelCount = VTX_FFPV_CHANNEL_COUNT, + .capability.powerCount = VTX_FFPV_POWER_COUNT, + .bandNames = (char **)ffpvBandNames, + .channelNames = (char **)ffpvChannelNames, + .powerNames = (char **)ffpvPowerNames, +}; + +bool vtxFuriousFPVInit(void) +{ + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_VTX_FFPV); + + if (portConfig) { + portOptions_t portOptions = 0; + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); + vtxSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_FFPV, NULL, NULL, 9600, MODE_RXTX, portOptions); + } + + if (!vtxSerialPort) { + return false; + } + + vtxCommonSetDevice(&impl_vtxDevice); + + ptRestart(ptGetHandle(impl_VtxProtocolThread)); + + return true; +} + +#endif \ No newline at end of file diff --git a/src/main/io/vtx_ffpv24g.h b/src/main/io/vtx_ffpv24g.h new file mode 100644 index 0000000000..d4110b68cf --- /dev/null +++ b/src/main/io/vtx_ffpv24g.h @@ -0,0 +1,51 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "platform.h" + +#include +#include +#include +#include + +#include "io/vtx.h" +#include "io/vtx_control.h" + +#define VTX_FFPV_BAND_COUNT 2 +#define VTX_FFPV_CHANNEL_COUNT 8 +#define VTX_FFPV_POWER_COUNT 4 + +extern const char * ffpvBandLetters; +extern const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1]; +extern const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1]; +extern const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1]; +extern const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT]; + +bool vtxFuriousFPVInit(void); +void ffpvSetBandAndChannel(uint8_t band, uint8_t channel); +void ffpvSetRFPowerByIndex(uint16_t index); + +vtxRunState_t * ffpvGetRuntimeState(void); \ No newline at end of file diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index aeda5748ad..bc86209927 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -63,13 +63,10 @@ serialPort_t *debugSerialPort = NULL; static serialPort_t *smartAudioSerialPort = NULL; -#if defined(USE_CMS) || defined(USE_VTX_COMMON) const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = { "---", "25 ", "200", "500", "800", }; -#endif -#ifdef USE_VTX_COMMON static const vtxVTable_t saVTable; // Forward static vtxDevice_t vtxSmartAudio = { .vTable = &saVTable, @@ -80,7 +77,6 @@ static vtxDevice_t vtxSmartAudio = { .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)saPowerNames, }; -#endif // SmartAudio command and response codes enum { @@ -340,10 +336,6 @@ static void saProcessResponse(uint8_t *buf, int len) } saDevicePrev = saDevice; -#ifdef USE_VTX_COMMON - // Todo: Update states in saVtxDevice? -#endif - #ifdef USE_CMS // Export current device status for CMS saCmsUpdate(); @@ -671,12 +663,7 @@ bool vtxSmartAudioInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { portOptions_t portOptions = SERIAL_BIDIR_NOPULL; -#if defined(USE_VTX_COMMON) portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); -#else - portOptions = SERIAL_BIDIR; -#endif - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } @@ -766,7 +753,6 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) } } -#ifdef USE_VTX_COMMON // Interface to common VTX API vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) @@ -893,7 +879,6 @@ static const vtxVTable_t saVTable = { .getPitMode = vtxSAGetPitMode, .getFrequency = vtxSAGetFreq, }; -#endif // VTX_COMMON #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index bbaa3eb7b1..f0f4c5382f 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,8 +25,6 @@ #include "platform.h" #include "build/debug.h" -#if defined(USE_VTX_COMMON) - #define VTX_STRING_BAND_COUNT 5 #define VTX_STRING_CHAN_COUNT 8 @@ -89,5 +87,3 @@ uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel) } return 0; } - -#endif diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 9a984c9926..c9c6b7694f 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -44,7 +44,7 @@ #include "io/vtx.h" #include "io/vtx_string.h" -#if defined(USE_CMS) || defined(USE_VTX_COMMON) +#if defined(USE_CMS) const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 }; @@ -54,7 +54,6 @@ const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = { }; #endif -#if defined(USE_VTX_COMMON) static const vtxVTable_t trampVTable; // forward static vtxDevice_t vtxTramp = { .vTable = &trampVTable, @@ -65,7 +64,6 @@ static vtxDevice_t vtxTramp = { .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)trampPowerNames, }; -#endif static serialPort_t *trampSerialPort = NULL; @@ -479,8 +477,6 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) } -#ifdef USE_VTX_COMMON - // Interface to common VTX API static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice) @@ -588,7 +584,6 @@ static const vtxVTable_t trampVTable = { .getFrequency = vtxTrampGetFreq, }; -#endif bool vtxTrampInit(void) { @@ -596,11 +591,7 @@ bool vtxTrampInit(void) if (portConfig) { portOptions_t portOptions = 0; -#if defined(USE_VTX_COMMON) portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); -#else - portOptions = SERIAL_BIDIR; -#endif trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); } @@ -609,9 +600,7 @@ bool vtxTrampInit(void) return false; } -#if defined(USE_VTX_COMMON) vtxCommonSetDevice(&vtxTramp); -#endif return true; } diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 8498fdc4b7..503ff098b3 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -111,6 +111,7 @@ #define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL +#undef USE_VTX_FFPV #undef USE_VTX_SMARTAUDIO // Disabled due to flash size #undef USE_VTX_TRAMP // Disabled due to flash size diff --git a/src/main/target/common.h b/src/main/target/common.h index 270818874e..82e40327f1 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -133,10 +133,10 @@ #define USE_PITOT_ADC //Enable VTX control -#define USE_VTX_COMMON #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP +#define USE_VTX_FFPV //Enable DST calculations #define RTC_AUTOMATIC_DST