From 58ed2ed590a80ba23216d67736d009295f4c793e Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 9 Jan 2017 22:09:32 +0900 Subject: [PATCH 01/26] Tramp prototype driver --- Makefile | 4 +- src/main/cms/cms_menu_builtin.c | 6 +- src/main/drivers/vtx_var.c | 24 ++++ src/main/drivers/vtx_var.h | 5 + src/main/drivers/vtxtab.c | 21 ++++ src/main/fc/fc_init.c | 5 + src/main/io/serial.h | 2 +- src/main/io/vtx_common.h | 35 ++++++ src/main/io/vtx_tramp.c | 195 ++++++++++++++++++++++++++++++++ src/main/io/vtx_tramp.h | 9 ++ src/main/target/common.h | 5 +- 11 files changed, 306 insertions(+), 5 deletions(-) create mode 100644 src/main/drivers/vtx_var.c create mode 100644 src/main/drivers/vtx_var.h create mode 100644 src/main/drivers/vtxtab.c create mode 100644 src/main/io/vtx_common.h create mode 100644 src/main/io/vtx_tramp.c create mode 100644 src/main/io/vtx_tramp.h diff --git a/Makefile b/Makefile index dcd122ec83..fbc7b1d6d1 100644 --- a/Makefile +++ b/Makefile @@ -610,7 +610,9 @@ HIGHEND_SRC = \ telemetry/mavlink.c \ telemetry/ibus.c \ sensors/esc_sensor.c \ - io/vtx_smartaudio.c + drivers/vtx_var.c \ + io/vtx_smartaudio.c \ + io/vtx_tramp.c SPEED_OPTIMISED_SRC := "" SIZE_OPTIMISED_SRC := "" diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index a0dc6b19ea..783864ffe1 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -48,6 +48,7 @@ // User supplied menus #include "io/vtx_smartaudio_cms.h" +#include "io/vtx_tramp.h" // Info @@ -97,7 +98,10 @@ static OSD_Entry menuFeaturesEntries[] = {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 #if defined(VTX_SMARTAUDIO) - {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, + {"VTX SA", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, +#endif +#if defined(VTX_TRAMP) + {"VTX TR", OME_Submenu, cmsMenuChange, &cmsx_menuVtxTramp, 0}, #endif #ifdef LED_STRIP {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, diff --git a/src/main/drivers/vtx_var.c b/src/main/drivers/vtx_var.c new file mode 100644 index 0000000000..3c2e543b61 --- /dev/null +++ b/src/main/drivers/vtx_var.c @@ -0,0 +1,24 @@ +#include +#include "vtx_var.h" + +const uint16_t vtx58FreqTable[5][8] = +{ + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A + { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B + { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E + { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark + { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand +}; + +const char * const vtx58BandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +const char * const vtx58ChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; diff --git a/src/main/drivers/vtx_var.h b/src/main/drivers/vtx_var.h new file mode 100644 index 0000000000..c2dec10199 --- /dev/null +++ b/src/main/drivers/vtx_var.h @@ -0,0 +1,5 @@ +#include + +extern const uint16_t vtx58FreqTable[5][8]; +extern const char * const vtx58BandNames[]; +extern const char * const vtx58ChanNames[]; diff --git a/src/main/drivers/vtxtab.c b/src/main/drivers/vtxtab.c new file mode 100644 index 0000000000..bb422c7314 --- /dev/null +++ b/src/main/drivers/vtxtab.c @@ -0,0 +1,21 @@ +static const uint16_t vtx58FreqTable[5][8] = +{ + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A + { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B + { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E + { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark + { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand +}; + +static const char * const vtx58BandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +static const char * const vtx58ChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2cf275de33..f86682bb66 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -92,6 +92,7 @@ #include "io/displayport_msp.h" #include "io/vtx.h" #include "io/vtx_smartaudio.h" +#include "io/vtx_tramp.h" #include "scheduler/scheduler.h" @@ -527,6 +528,10 @@ void init(void) smartAudioInit(); #endif +#ifdef VTX_TRAMP + trampInit(); +#endif + // start all timers // TODO - not implemented yet timerStart(); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 6bbfb0420f..b8371cc322 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -38,7 +38,7 @@ typedef enum { FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 FUNCTION_ESC_SENSOR = (1 << 10), // 1024 FUNCTION_VTX_CONTROL = (1 << 11), // 2048 - FUNCTION_TELEMETRY_IBUS = (1 << 12) // 4096 + FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h new file mode 100644 index 0000000000..32748db437 --- /dev/null +++ b/src/main/io/vtx_common.h @@ -0,0 +1,35 @@ + +struct vtxVTable_s; +typedef struct vtxDevice_s { + const struct vtxVTable_s *vTable; + uint8_t numBand; + uint8_t numChan; + uint8_t numPower; + + uint16_t *freqTable; + + // Config variable? + uint8_t opModel; // Power up in: 0 = transmitting, 1 = pit mode + + // CMS only? + uint8_t curBand; + uint8_t curChan; + uint8_t curPower; + uint8_t curPitState; // 0 = PIT, 1 = non-PIT + + // CMS only? + char *bandNames; + char *chanNames; + char *powerNames; +} vtxDevice_t; + +typedef struct vtxVTable_s { + void (*setBandChan)(uint8_t band, uint8_t chan); + void (*setFreq)(uint16_t freq); + void (*setRFPower)(uint8_t level); // 0 = OFF, 1~ = device dep. + void (*setPitmode)(uint8_t onoff); +} vtxVTable_t; + +// PIT mode is defined as LOWEST POSSIBLE RF POWER. +// - It can be a dedicated mode, or lowest RF power possible. +// - It is *NOT* RF on/off control. diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c new file mode 100644 index 0000000000..f2e07ab622 --- /dev/null +++ b/src/main/io/vtx_tramp.c @@ -0,0 +1,195 @@ +/* + * 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 . + */ + +/* Created by jflyper */ + +#include +#include +#include + +#include "platform.h" + +#ifdef VTX_TRAMP + +#include "build/debug.h" + +#include "common/utils.h" + +#include "io/serial.h" +#include "drivers/serial.h" +#include "drivers/vtx_var.h" +#include "io/vtx_tramp.h" + +static serialPort_t *trampSerialPort = NULL; + +static uint8_t trampCmdBuffer[16]; + +void trampClearBuffer(uint8_t *buf) +{ + for (int i = 0 ; i < 16 ; i++) { + buf[i] = 0; + } +} + +void trampPrepareBuffer(uint8_t *buf, uint8_t cmd) +{ + trampClearBuffer(buf); + + trampCmdBuffer[0] = 15; + trampCmdBuffer[1] = cmd; +} + +void trampWriteBuf(uint8_t *buf) +{ + serialWriteBuf(trampSerialPort, buf, 16); +} + +void trampChecksum(uint8_t *buf) +{ + uint8_t cksum = 0; + + for (int i = 0 ; i < 14 ; i++) + cksum += buf[i]; + + buf[14] = cksum; +} + +void trampSetFreq(uint16_t freq) +{ + if (!trampSerialPort) + return; + + trampPrepareBuffer(trampCmdBuffer, 'F'); + trampCmdBuffer[2] = freq & 0xff; + trampCmdBuffer[3] = (freq >> 8) & 0xff; + trampChecksum(trampCmdBuffer); + trampWriteBuf(trampCmdBuffer); +} + +void trampSetBandChan(uint8_t band, uint8_t chan) +{ + if (!trampSerialPort) + return; + +debug[0] = band; +debug[1] = chan; +debug[2] = vtx58FreqTable[band - 1][chan - 1]; + + trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); +} + +void trampSetRFPower(uint8_t level) +{ + if (!trampSerialPort) + return; + + trampPrepareBuffer(trampCmdBuffer, 'F'); + trampCmdBuffer[2] = level; + trampChecksum(trampCmdBuffer); + trampWriteBuf(trampCmdBuffer); +} + +void trampSetPitmode(uint8_t onoff) +{ + if (!trampSerialPort) + return; + + trampPrepareBuffer(trampCmdBuffer, 'I'); + trampCmdBuffer[2] = onoff; + trampChecksum(trampCmdBuffer); + trampWriteBuf(trampCmdBuffer); +} + +bool trampInit() +{ + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); + + if (portConfig) { + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, 0); // MODE_TX possible? + } + + if (!trampSerialPort) { + return false; + } + + return true; +} + +#ifdef CMS +#include "cms/cms.h" +#include "cms/cms_types.h" + +uint8_t trampCmsBand = 1; +uint8_t trampCmsChan = 1; + +static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames, NULL }; + +static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; + +static long trampCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + trampSetBandChan(trampCmsBand, trampCmsChan); + + return MENU_CHAIN_BACK; +} + +static OSD_Entry trampCmsMenuCommenceEntries[] = { + { "CONFIRM", OME_Label, NULL, NULL, 0 }, + { "YES", OME_Funcall, trampCmsCommence, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu trampCmsMenuCommence = { + .GUARD_text = "XVTXTRC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = trampCmsMenuCommenceEntries, +}; + +static OSD_Entry trampMenuEntries[] = +{ + { "- TRAMP -", OME_Label, NULL, NULL, 0 }, + + //{ "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "BAND", OME_TAB, NULL, &trampCmsEntBand, 0 }, + { "CHAN", OME_TAB, NULL, &trampCmsEntChan, 0 }, + //{ "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, + //{ "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, + //{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuVtxTramp = { + .GUARD_text = "XVTXTR", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = trampMenuEntries, +}; +#endif + +#endif // VTX_TRAMP diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h new file mode 100644 index 0000000000..4fcdaf40d1 --- /dev/null +++ b/src/main/io/vtx_tramp.h @@ -0,0 +1,9 @@ +#pragma once + +bool trampInit(); + +#ifdef CMS +#include "cms/cms.h" +#include "cms/cms_types.h" +extern CMS_Menu cmsx_menuVtxTramp; +#endif diff --git a/src/main/target/common.h b/src/main/target/common.h index f71963ee9b..dcc59bf32d 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -103,8 +103,9 @@ #define TELEMETRY_IBUS #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS -#define VTX_CONTROL -#define VTX_SMARTAUDIO +//#define VTX_CONTROL +//#define VTX_SMARTAUDIO +#define VTX_TRAMP #define USE_SENSOR_NAMES #else #define SKIP_CLI_COMMAND_HELP From 6515ae3490a5eb56d1df640bd132e3c5eae16471 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 11 Jan 2017 01:25:07 +0900 Subject: [PATCH 02/26] Added power and pit mode --- Makefile | 1 + src/main/io/vtx_tramp.c | 47 ++++++++++++++++++++++++++++++++++++----- 2 files changed, 43 insertions(+), 5 deletions(-) diff --git a/Makefile b/Makefile index fbc7b1d6d1..ae73af495e 100644 --- a/Makefile +++ b/Makefile @@ -718,6 +718,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ io/vtx_smartaudio.c + # io/vtx_tramp.c # makes binary bigger!? endif #F3 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f2e07ab622..1ce79d9c1e 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -92,13 +92,14 @@ debug[2] = vtx58FreqTable[band - 1][chan - 1]; trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); } -void trampSetRFPower(uint8_t level) +void trampSetRFPower(uint16_t level) { if (!trampSerialPort) return; - trampPrepareBuffer(trampCmdBuffer, 'F'); - trampCmdBuffer[2] = level; + trampPrepareBuffer(trampCmdBuffer, 'P'); + trampCmdBuffer[2] = level & 0xff; + trampCmdBuffer[3] = (level >> 8) & 0xff; trampChecksum(trampCmdBuffer); trampWriteBuf(trampCmdBuffer); } @@ -133,19 +134,54 @@ bool trampInit() #include "cms/cms.h" #include "cms/cms_types.h" +uint8_t trampCmsPitmode = 0; uint8_t trampCmsBand = 1; uint8_t trampCmsChan = 1; +uint16_t trampCmsFreqRef; static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames, NULL }; static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; +static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; + +static const char * const trampCmsPowerNames[] = { + "25", "100", "200", "400", "600" +}; + +static const uint16_t trampCmsPowerTable[] = { + 25, 100, 200, 400, 600 +}; + +static uint8_t trampCmsPower = 0; + +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampCmsPowerNames, NULL }; + +static const char * const trampCmsPitmodeNames[] = { + "OFF", "ON " +}; + +static OSD_TAB_t trampCmsEntPitmode = { &trampCmsPitmode, 2, trampCmsPitmodeNames, NULL }; + +static long trampCmsSetPitmode(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + trampSetPitmode(trampCmsPitmode); + + return 0; +} + static long trampCmsCommence(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); trampSetBandChan(trampCmsBand, trampCmsChan); + trampSetRFPower(trampCmsPowerTable[trampCmsPower]); + + trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; return MENU_CHAIN_BACK; } @@ -171,10 +207,11 @@ static OSD_Entry trampMenuEntries[] = { "- TRAMP -", OME_Label, NULL, NULL, 0 }, //{ "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, { "BAND", OME_TAB, NULL, &trampCmsEntBand, 0 }, { "CHAN", OME_TAB, NULL, &trampCmsEntChan, 0 }, - //{ "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, - //{ "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, + { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, //{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, From eec475078c9792763c48c2bb819ee9d5333a42d9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 11 Jan 2017 04:45:05 +0900 Subject: [PATCH 03/26] Removed debug --- src/main/io/vtx_tramp.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 1ce79d9c1e..6e5f5af384 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -85,10 +85,6 @@ void trampSetBandChan(uint8_t band, uint8_t chan) if (!trampSerialPort) return; -debug[0] = band; -debug[1] = chan; -debug[2] = vtx58FreqTable[band - 1][chan - 1]; - trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); } From 74d175edfb7a3262a4de11b3c2727d673744ad35 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 11 Jan 2017 14:52:43 +0900 Subject: [PATCH 04/26] Changes per @ledvinap's commend + some debugging --- src/main/drivers/vtxtab.c | 21 ----------- src/main/io/vtx_tramp.c | 74 ++++++++++++++------------------------- 2 files changed, 27 insertions(+), 68 deletions(-) delete mode 100644 src/main/drivers/vtxtab.c diff --git a/src/main/drivers/vtxtab.c b/src/main/drivers/vtxtab.c deleted file mode 100644 index bb422c7314..0000000000 --- a/src/main/drivers/vtxtab.c +++ /dev/null @@ -1,21 +0,0 @@ -static const uint16_t vtx58FreqTable[5][8] = -{ - { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A - { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B - { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E - { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark - { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand -}; - -static const char * const vtx58BandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -static const char * const vtx58ChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 6e5f5af384..ee4dff6807 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -20,6 +20,7 @@ #include #include #include +#include #include "platform.h" @@ -38,77 +39,53 @@ static serialPort_t *trampSerialPort = NULL; static uint8_t trampCmdBuffer[16]; -void trampClearBuffer(uint8_t *buf) -{ - for (int i = 0 ; i < 16 ; i++) { - buf[i] = 0; - } -} - -void trampPrepareBuffer(uint8_t *buf, uint8_t cmd) -{ - trampClearBuffer(buf); - - trampCmdBuffer[0] = 15; - trampCmdBuffer[1] = cmd; -} - -void trampWriteBuf(uint8_t *buf) +static void trampWriteBuf(uint8_t *buf) { serialWriteBuf(trampSerialPort, buf, 16); } -void trampChecksum(uint8_t *buf) +static uint8_t trampChecksum(uint8_t *buf) { uint8_t cksum = 0; for (int i = 0 ; i < 14 ; i++) cksum += buf[i]; - buf[14] = cksum; + return cksum; +} + +void trampCmdU16(uint8_t cmd, uint16_t param) +{ + if (!trampSerialPort) + return; + + memset(trampCmdBuffer, 0, ARRAYLEN(trampCmdBuffer)); + trampCmdBuffer[0] = 15; + trampCmdBuffer[1] = cmd; + trampCmdBuffer[2] = param & 0xff; + trampCmdBuffer[3] = (param >> 8) & 0xff; + trampCmdBuffer[14] = trampChecksum(trampCmdBuffer); + trampWriteBuf(trampCmdBuffer); } void trampSetFreq(uint16_t freq) { - if (!trampSerialPort) - return; - - trampPrepareBuffer(trampCmdBuffer, 'F'); - trampCmdBuffer[2] = freq & 0xff; - trampCmdBuffer[3] = (freq >> 8) & 0xff; - trampChecksum(trampCmdBuffer); - trampWriteBuf(trampCmdBuffer); + trampCmdU16('F', freq); } void trampSetBandChan(uint8_t band, uint8_t chan) { - if (!trampSerialPort) - return; - - trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); + trampCmdU16('F', vtx58FreqTable[band - 1][chan - 1]); } void trampSetRFPower(uint16_t level) { - if (!trampSerialPort) - return; - - trampPrepareBuffer(trampCmdBuffer, 'P'); - trampCmdBuffer[2] = level & 0xff; - trampCmdBuffer[3] = (level >> 8) & 0xff; - trampChecksum(trampCmdBuffer); - trampWriteBuf(trampCmdBuffer); + trampCmdU16('P', level); } void trampSetPitmode(uint8_t onoff) { - if (!trampSerialPort) - return; - - trampPrepareBuffer(trampCmdBuffer, 'I'); - trampCmdBuffer[2] = onoff; - trampChecksum(trampCmdBuffer); - trampWriteBuf(trampCmdBuffer); + trampCmdU16('I', (uint16_t)onoff); } bool trampInit() @@ -116,7 +93,7 @@ bool trampInit() serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, 0); // MODE_TX possible? + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, 0); } if (!trampSerialPort) { @@ -174,8 +151,11 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + // XXX Does Tramp handles back-to-back commands properly!? trampSetBandChan(trampCmsBand, trampCmsChan); - trampSetRFPower(trampCmsPowerTable[trampCmsPower]); + + // Test without back-to-back commands. + // trampSetRFPower(trampCmsPowerTable[trampCmsPower]); trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; From 0c026a237ad2247fcf62f71215fffd14d1f432b9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 12 Jan 2017 03:47:28 +0900 Subject: [PATCH 05/26] Checksum range changed from 1 --- src/main/io/vtx_tramp.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index ee4dff6807..f65bf51624 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -44,12 +44,12 @@ static void trampWriteBuf(uint8_t *buf) serialWriteBuf(trampSerialPort, buf, 16); } -static uint8_t trampChecksum(uint8_t *buf) +static uint8_t trampChecksum(uint8_t *trampBuf) { uint8_t cksum = 0; - for (int i = 0 ; i < 14 ; i++) - cksum += buf[i]; + for (int i = 1 ; i < 14 ; i++) + cksum += trampBuf[i]; return cksum; } From 6550d738db1efe3bafc45548a832a7bae48fcca1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 12 Jan 2017 16:53:22 +0900 Subject: [PATCH 06/26] Some enhancements --- Makefile | 1 + src/main/drivers/vtx_var.c | 2 + src/main/drivers/vtx_var.h | 3 + src/main/fc/fc_tasks.c | 6 +- src/main/io/vtx_common.c | 52 +++++++++ src/main/io/vtx_common.h | 2 + src/main/io/vtx_tramp.c | 213 +++++++++++++++++++++++++++++++++++-- src/main/io/vtx_tramp.h | 1 + src/main/target/common.h | 2 +- 9 files changed, 269 insertions(+), 13 deletions(-) create mode 100644 src/main/io/vtx_common.c diff --git a/Makefile b/Makefile index ae73af495e..c04f22854c 100644 --- a/Makefile +++ b/Makefile @@ -611,6 +611,7 @@ HIGHEND_SRC = \ telemetry/ibus.c \ sensors/esc_sensor.c \ drivers/vtx_var.c \ + io/vtx_common.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c diff --git a/src/main/drivers/vtx_var.c b/src/main/drivers/vtx_var.c index 3c2e543b61..8be89c4300 100644 --- a/src/main/drivers/vtx_var.c +++ b/src/main/drivers/vtx_var.c @@ -19,6 +19,8 @@ const char * const vtx58BandNames[] = { "RACEBAND", }; +const char vtx58BandLetter[] = "-ABEFR"; + const char * const vtx58ChanNames[] = { "-", "1", "2", "3", "4", "5", "6", "7", "8", }; diff --git a/src/main/drivers/vtx_var.h b/src/main/drivers/vtx_var.h index c2dec10199..c8e7cdf2dc 100644 --- a/src/main/drivers/vtx_var.h +++ b/src/main/drivers/vtx_var.h @@ -1,5 +1,8 @@ +#pragma once + #include extern const uint16_t vtx58FreqTable[5][8]; extern const char * const vtx58BandNames[]; extern const char * const vtx58ChanNames[]; +extern const char vtx58BandLetter[]; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 6442870268..b746f0252e 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -53,6 +53,7 @@ #include "io/serial.h" #include "io/transponder_ir.h" #include "io/vtx_smartaudio.h" +#include "io/vtx_tramp.h" #include "msp/msp_serial.h" @@ -215,6 +216,9 @@ void taskVtxControl(uint32_t currentTime) #ifdef VTX_SMARTAUDIO smartAudioProcess(currentTime); #endif +#ifdef VTX_TRAMP + trampProcess(currentTime); +#endif } #endif @@ -300,7 +304,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_STACK_CHECK, true); #endif #ifdef VTX_CONTROL -#ifdef VTX_SMARTAUDIO +#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif diff --git a/src/main/io/vtx_common.c b/src/main/io/vtx_common.c new file mode 100644 index 0000000000..62b9c00f0f --- /dev/null +++ b/src/main/io/vtx_common.c @@ -0,0 +1,52 @@ +/* + * 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 . + */ + +/* Created by jflyper */ + +#include +#include +#include +#include + +#include "platform.h" +#include "build/debug.h" + +#if defined(VTX_TRAMP) +#include "drivers/vtx_var.h" + +bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan) +{ + uint8_t band; + uint8_t chan; + + for (band = 0 ; band < 5 ; band++) { + for (chan = 0 ; chan < 8 ; chan++) { + if (vtx58FreqTable[band][chan] == freq) { + *pBand = band + 1; + *pChan = chan + 1; + return true; + } + } + } + + *pBand = 0; + *pChan = 0; + + return false; +} + +#endif diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h index 32748db437..ed12d19d19 100644 --- a/src/main/io/vtx_common.h +++ b/src/main/io/vtx_common.h @@ -33,3 +33,5 @@ typedef struct vtxVTable_s { // PIT mode is defined as LOWEST POSSIBLE RF POWER. // - It can be a dedicated mode, or lowest RF power possible. // - It is *NOT* RF on/off control. + +bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f65bf51624..cbac140216 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -29,15 +29,39 @@ #include "build/debug.h" #include "common/utils.h" +#include "common/printf.h" #include "io/serial.h" #include "drivers/serial.h" #include "drivers/vtx_var.h" #include "io/vtx_tramp.h" +#include "io/vtx_common.h" static serialPort_t *trampSerialPort = NULL; -static uint8_t trampCmdBuffer[16]; +static uint8_t trampReqBuffer[16]; +static uint8_t trampRespBuffer[16]; + +typedef enum { + TRAMP_STATUS_BAD_DEVICE = -1, + TRAMP_STATUS_OFFLINE = 0, + TRAMP_STATUS_ONLINE +} trampStatus_e; + +trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; + +uint32_t trampRFFreqMin; +uint32_t trampRFFreqMax; +uint32_t trampRFPowerMax; + +uint32_t trampCurFreq = 0; +uint8_t trampCurBand = 0; +uint8_t trampCurChan = 0; +uint16_t trampCurPower = 0; + +#ifdef CMS +void trampCmsUpdateStatusString(void); // Forward +#endif static void trampWriteBuf(uint8_t *buf) { @@ -59,13 +83,13 @@ void trampCmdU16(uint8_t cmd, uint16_t param) if (!trampSerialPort) return; - memset(trampCmdBuffer, 0, ARRAYLEN(trampCmdBuffer)); - trampCmdBuffer[0] = 15; - trampCmdBuffer[1] = cmd; - trampCmdBuffer[2] = param & 0xff; - trampCmdBuffer[3] = (param >> 8) & 0xff; - trampCmdBuffer[14] = trampChecksum(trampCmdBuffer); - trampWriteBuf(trampCmdBuffer); + memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); + trampReqBuffer[0] = 15; + trampReqBuffer[1] = cmd; + trampReqBuffer[2] = param & 0xff; + trampReqBuffer[3] = (param >> 8) & 0xff; + trampReqBuffer[14] = trampChecksum(trampReqBuffer); + trampWriteBuf(trampReqBuffer); } void trampSetFreq(uint16_t freq) @@ -88,12 +112,38 @@ void trampSetPitmode(uint8_t onoff) trampCmdU16('I', (uint16_t)onoff); } +static uint8_t trampPendingQuery = 0; // XXX Assume no code/resp == 0 + +void trampQuery(uint8_t cmd) +{ + trampPendingQuery = cmd; + trampCmdU16(cmd, 0); +} + +void trampQueryR(void) +{ + trampQuery('r'); +} + +void trampQueryV(void) +{ + trampQuery('v'); +} + +void trampQueryS(void) +{ + trampQuery('s'); +} + +//#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) +#define TRAMP_SERIAL_OPTIONS (0) + bool trampInit() { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, 0); + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); } if (!trampSerialPort) { @@ -103,10 +153,151 @@ bool trampInit() return true; } +void trampHandleResponse(void) +{ + uint8_t respCode = trampRespBuffer[1]; + + switch (respCode) { + case 'r': + trampRFFreqMin = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); + trampStatus = TRAMP_STATUS_ONLINE; + break; + + case 'v': + trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + break; + + case 's': + break; + } + + if (trampPendingQuery == respCode) + trampPendingQuery = 0; +} + +static bool trampIsValidResponseCode(uint8_t code) +{ + if (code == 'r' || code == 'v' || code == 's') + return true; + else + return false; +} + +typedef enum { + S_WAIT_LEN = 0, // Waiting for a packet len + S_WAIT_CODE, // Waiting for a response code + S_DATA, // Waiting for rest of the packet. +} trampReceiveState_e; + +static trampReceiveState_e trampReceiveState = S_WAIT_LEN; +static int trampReceivePos = 0; +static uint32_t trampFrameStartUs = 0; + +// Frame timeout. An actual frame (16B) takes only 16.6 msec, +// but a frame arrival may span two scheduling intervals (200msec * 2). +// Effectively same as waiting for a next trampProcess() to run. + +#define TRAMP_FRAME_TIMO_US (200 * 1000) + +void trampReceive(uint32_t currentTimeUs) +{ + if (!trampSerialPort) + return; + + if ((trampReceiveState != S_WAIT_LEN) && (currentTimeUs - trampFrameStartUs > TRAMP_FRAME_TIMO_US)) { + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; + } + + while (serialRxBytesWaiting(trampSerialPort)) { + uint8_t c = serialRead(trampSerialPort); + trampRespBuffer[trampReceivePos++] = c; + + switch(c) { + case S_WAIT_LEN: + if (c == 0x0F) { + trampReceiveState = S_WAIT_CODE; + trampFrameStartUs = currentTimeUs; + } + break; + + case S_WAIT_CODE: + if (trampIsValidResponseCode(c)) { + trampReceiveState = S_DATA; + } else { + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; + } + break; + + case S_DATA: + if (trampReceivePos == 16) { + uint8_t cksum = trampChecksum(trampRespBuffer); + if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) + trampHandleResponse(); + + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; + } + break; + } + } +} + +void trampProcess(uint32_t currentTimeUs) +{ + static uint32_t lastQueryRTimeUs = 0; + + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + return; + +debug[0]++; + trampReceive(currentTimeUs); + + if (trampStatus == TRAMP_STATUS_OFFLINE) { + if (currentTimeUs - lastQueryRTimeUs > 1000 * 1000) { + trampQueryR(); + lastQueryRTimeUs = currentTimeUs; + } + } else if (trampPendingQuery) { + trampQuery(trampPendingQuery); + } else { + trampQueryV(); + } + +#ifdef CMS + trampCmsUpdateStatusString(); +#endif +} + #ifdef CMS #include "cms/cms.h" #include "cms/cms_types.h" +char trampCmsStatusString[16]; + +void trampCmsUpdateStatusString(void) +{ + trampCmsStatusString[0] = '*'; + trampCmsStatusString[1] = ' '; + trampCmsStatusString[3] = vtx58BandLetter[trampCurBand]; + trampCmsStatusString[4] = vtx58ChanNames[trampCurChan][0]; + trampCmsStatusString[5] = ' '; + + if (trampCurFreq) + tfp_sprintf(&trampCmsStatusString[6], "%4d", trampCurFreq); + else + tfp_sprintf(&trampCmsStatusString[6], "----"); + + if (trampCurPower) + tfp_sprintf(&trampCmsStatusString[10], " %3d", trampCurPower); + else + tfp_sprintf(&trampCmsStatusString[10], " ---"); +} + uint8_t trampCmsPitmode = 0; uint8_t trampCmsBand = 1; uint8_t trampCmsChan = 1; @@ -182,8 +373,8 @@ static OSD_Entry trampMenuEntries[] = { { "- TRAMP -", OME_Label, NULL, NULL, 0 }, - //{ "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, + { "", OME_Label, NULL, trampCmsStatusString, DYNAMIC }, + { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, { "BAND", OME_TAB, NULL, &trampCmsEntBand, 0 }, { "CHAN", OME_TAB, NULL, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 4fcdaf40d1..0d08d0d293 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -1,6 +1,7 @@ #pragma once bool trampInit(); +void trampProcess(uint32_t currentTimeUs); #ifdef CMS #include "cms/cms.h" diff --git a/src/main/target/common.h b/src/main/target/common.h index dcc59bf32d..848570746d 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -103,7 +103,7 @@ #define TELEMETRY_IBUS #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS -//#define VTX_CONTROL +#define VTX_CONTROL //#define VTX_SMARTAUDIO #define VTX_TRAMP #define USE_SENSOR_NAMES From 195d3a3cf369f27b9650cc769c99ebe8010dceba Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 12 Jan 2017 16:56:58 +0900 Subject: [PATCH 07/26] Serial option to production --- src/main/io/vtx_tramp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index cbac140216..ca9acab15a 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -135,8 +135,8 @@ void trampQueryS(void) trampQuery('s'); } -//#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) -#define TRAMP_SERIAL_OPTIONS (0) +#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) +//#define TRAMP_SERIAL_OPTIONS (0) // For debugging with tramp emulator bool trampInit() { From 9bca2b88d0f0c276cf7325f0b2ace8d020f4fa9c Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 12 Jan 2017 17:57:30 +0900 Subject: [PATCH 08/26] Fixes and extensions --- src/main/drivers/vtx_var.c | 2 +- src/main/io/vtx_common.h | 36 +------------------------- src/main/io/vtx_smartaudio.c | 40 ++++++----------------------- src/main/io/vtx_tramp.c | 50 ++++++++++++++++++++++-------------- 4 files changed, 41 insertions(+), 87 deletions(-) diff --git a/src/main/drivers/vtx_var.c b/src/main/drivers/vtx_var.c index 8be89c4300..f541b4a6a5 100644 --- a/src/main/drivers/vtx_var.c +++ b/src/main/drivers/vtx_var.c @@ -3,7 +3,7 @@ const uint16_t vtx58FreqTable[5][8] = { - { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h index ed12d19d19..8c260279b3 100644 --- a/src/main/io/vtx_common.h +++ b/src/main/io/vtx_common.h @@ -1,37 +1,3 @@ - -struct vtxVTable_s; -typedef struct vtxDevice_s { - const struct vtxVTable_s *vTable; - uint8_t numBand; - uint8_t numChan; - uint8_t numPower; - - uint16_t *freqTable; - - // Config variable? - uint8_t opModel; // Power up in: 0 = transmitting, 1 = pit mode - - // CMS only? - uint8_t curBand; - uint8_t curChan; - uint8_t curPower; - uint8_t curPitState; // 0 = PIT, 1 = non-PIT - - // CMS only? - char *bandNames; - char *chanNames; - char *powerNames; -} vtxDevice_t; - -typedef struct vtxVTable_s { - void (*setBandChan)(uint8_t band, uint8_t chan); - void (*setFreq)(uint16_t freq); - void (*setRFPower)(uint8_t level); // 0 = OFF, 1~ = device dep. - void (*setPitmode)(uint8_t onoff); -} vtxVTable_t; - -// PIT mode is defined as LOWEST POSSIBLE RF POWER. -// - It can be a dedicated mode, or lowest RF power possible. -// - It is *NOT* RF on/off control. +#pragma once bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 2fbe947435..8ece43894b 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -33,6 +33,7 @@ #include "common/utils.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/vtx_var.h" #include "io/serial.h" #include "io/vtx_smartaudio.h" @@ -130,18 +131,6 @@ static smartAudioStat_t saStat = { .badcode = 0, }; -// The band/chan to frequency table -// XXX Should really be consolidated among different vtx drivers -static const uint16_t saFreqTable[5][8] = -{ - { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A - { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B - { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E - { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark - { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand -}; -// XXX Should frequencies not usable in locked state be detected? - typedef struct saPowerTable_s { int rfpower; int16_t valueV1; @@ -767,9 +756,9 @@ void saCmsUpdate(void) saCmsBand = (saDevice.chan / 8) + 1; saCmsChan = (saDevice.chan % 8) + 1; - saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsFreqRef = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; - saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsDeviceFreq = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { saCmsRFState = SACMS_TXMODE_ACTIVE; @@ -828,7 +817,7 @@ else tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq); else tfp_sprintf(&saCmsStatusString[5], "%4d", - saFreqTable[saDevice.chan / 8][saDevice.chan % 8]); + vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]); saCmsStatusString[9] = ' '; @@ -868,7 +857,7 @@ dprintf(("saCmsConfigBand: band req %d ", saCmsBand)); if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); - saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1]; + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; return 0; } @@ -893,7 +882,7 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); - saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1]; + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; return 0; } @@ -994,22 +983,9 @@ static CMS_Menu saCmsMenuStats = { .entries = saCmsMenuStatsEntries }; -static const char * const saCmsBandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; +static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL }; -static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, &saCmsBandNames[0], NULL }; - -static const char * const saCmsChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; - -static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &saCmsChanNames[0], NULL }; +static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChanNames[0], NULL }; static const char * const saCmsPowerNames[] = { "---", diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index ca9acab15a..1253d9cb53 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -57,7 +57,9 @@ uint32_t trampRFPowerMax; uint32_t trampCurFreq = 0; uint8_t trampCurBand = 0; uint8_t trampCurChan = 0; -uint16_t trampCurPower = 0; +uint16_t trampCurPower = 0; // Actual transmitting power +uint16_t trampCurConfigPower = 0; // Configured transmitting power +int16_t trampCurTemp = 0; #ifdef CMS void trampCmsUpdateStatusString(void); // Forward @@ -167,10 +169,13 @@ void trampHandleResponse(void) case 'v': trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); break; case 's': + trampCurTemp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); break; } @@ -207,7 +212,7 @@ void trampReceive(uint32_t currentTimeUs) if (!trampSerialPort) return; - if ((trampReceiveState != S_WAIT_LEN) && (currentTimeUs - trampFrameStartUs > TRAMP_FRAME_TIMO_US)) { + if ((trampReceiveState != S_WAIT_LEN) && cmp32(currentTimeUs, trampFrameStartUs > TRAMP_FRAME_TIMO_US)) { trampReceiveState = S_WAIT_LEN; trampReceivePos = 0; } @@ -216,7 +221,7 @@ void trampReceive(uint32_t currentTimeUs) uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; - switch(c) { + switch(trampReceiveState) { case S_WAIT_LEN: if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; @@ -254,18 +259,18 @@ void trampProcess(uint32_t currentTimeUs) if (trampStatus == TRAMP_STATUS_BAD_DEVICE) return; -debug[0]++; trampReceive(currentTimeUs); if (trampStatus == TRAMP_STATUS_OFFLINE) { - if (currentTimeUs - lastQueryRTimeUs > 1000 * 1000) { + if (cmp32(currentTimeUs, lastQueryRTimeUs) > 1000 * 1000) { trampQueryR(); lastQueryRTimeUs = currentTimeUs; } - } else if (trampPendingQuery) { - trampQuery(trampPendingQuery); - } else { - trampQueryV(); + } else if (trampReceiveState == S_WAIT_LEN) { + if (trampPendingQuery) + trampQuery(trampPendingQuery); + else + trampQueryV(); } #ifdef CMS @@ -277,25 +282,29 @@ debug[0]++; #include "cms/cms.h" #include "cms/cms_types.h" -char trampCmsStatusString[16]; + +char trampCmsStatusString[31] = "- -- ---- ----"; +// m bc ffff tppp +// 01234567890123 void trampCmsUpdateStatusString(void) { trampCmsStatusString[0] = '*'; trampCmsStatusString[1] = ' '; - trampCmsStatusString[3] = vtx58BandLetter[trampCurBand]; - trampCmsStatusString[4] = vtx58ChanNames[trampCurChan][0]; - trampCmsStatusString[5] = ' '; + trampCmsStatusString[2] = vtx58BandLetter[trampCurBand]; + trampCmsStatusString[3] = vtx58ChanNames[trampCurChan][0]; + trampCmsStatusString[4] = ' '; if (trampCurFreq) - tfp_sprintf(&trampCmsStatusString[6], "%4d", trampCurFreq); + tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq); else - tfp_sprintf(&trampCmsStatusString[6], "----"); + tfp_sprintf(&trampCmsStatusString[5], "----"); - if (trampCurPower) - tfp_sprintf(&trampCmsStatusString[10], " %3d", trampCurPower); + if (trampCurPower) { + tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampCurPower == trampCurConfigPower) ? ' ' : '*', trampCurPower); + } else - tfp_sprintf(&trampCmsStatusString[10], " ---"); + tfp_sprintf(&trampCmsStatusString[9], " ---"); } uint8_t trampCmsPitmode = 0; @@ -319,7 +328,9 @@ static const uint16_t trampCmsPowerTable[] = { static uint8_t trampCmsPower = 0; -static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampCmsPowerNames, NULL }; +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NULL }; + +static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; static const char * const trampCmsPitmodeNames[] = { "OFF", "ON " @@ -379,6 +390,7 @@ static OSD_Entry trampMenuEntries[] = { "CHAN", OME_TAB, NULL, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, + { "TEMP", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, //{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, From 5bbf4e13c111833ed9d4792918792fd7b76e2b39 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 13 Jan 2017 02:35:31 +0900 Subject: [PATCH 09/26] Fixed I (PIT) command parameter --- src/main/io/vtx_tramp.c | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 1253d9cb53..18316bc715 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -111,7 +111,7 @@ void trampSetRFPower(uint16_t level) void trampSetPitmode(uint8_t onoff) { - trampCmdU16('I', (uint16_t)onoff); + trampCmdU16('I', onoff ? 0x0100 : 0); } static uint8_t trampPendingQuery = 0; // XXX Assume no code/resp == 0 @@ -304,7 +304,7 @@ void trampCmsUpdateStatusString(void) tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampCurPower == trampCurConfigPower) ? ' ' : '*', trampCurPower); } else - tfp_sprintf(&trampCmsStatusString[9], " ---"); + tfp_sprintf(&trampCmsStatusString[9], " ----"); } uint8_t trampCmsPitmode = 0; @@ -318,6 +318,17 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; +static long trampCmsUpdateFreqRef(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (trampCmsBand > 0 && trampCmsChan > 0) + trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; + + return 0; +} + static const char * const trampCmsPowerNames[] = { "25", "100", "200", "400", "600" }; @@ -353,11 +364,12 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - // XXX Does Tramp handles back-to-back commands properly!? trampSetBandChan(trampCmsBand, trampCmsChan); + // XXX Does Tramp handles back-to-back commands properly!? // Test without back-to-back commands. - // trampSetRFPower(trampCmsPowerTable[trampCmsPower]); + + trampSetRFPower(trampCmsPowerTable[trampCmsPower]); trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; @@ -386,8 +398,8 @@ static OSD_Entry trampMenuEntries[] = { "", OME_Label, NULL, trampCmsStatusString, DYNAMIC }, { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, - { "BAND", OME_TAB, NULL, &trampCmsEntBand, 0 }, - { "CHAN", OME_TAB, NULL, &trampCmsEntChan, 0 }, + { "BAND", OME_TAB, trampCmsUpdateFreqRef, &trampCmsEntBand, 0 }, + { "CHAN", OME_TAB, trampCmsUpdateFreqRef, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, { "TEMP", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, From 3d9a82e4aeda08bff2e1b191c599927c88be6945 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 13 Jan 2017 02:58:58 +0900 Subject: [PATCH 10/26] Disable trampHandleResponse() instead of trampReceive() --- src/main/io/vtx_tramp.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 18316bc715..0d1664f3a2 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -226,6 +226,8 @@ void trampReceive(uint32_t currentTimeUs) if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; trampFrameStartUs = currentTimeUs; + } else { + trampReceivePos = 0; } break; @@ -241,8 +243,9 @@ void trampReceive(uint32_t currentTimeUs) case S_DATA: if (trampReceivePos == 16) { uint8_t cksum = trampChecksum(trampRespBuffer); - if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) - trampHandleResponse(); + if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { + // trampHandleResponse(); + } trampReceiveState = S_WAIT_LEN; trampReceivePos = 0; @@ -259,7 +262,7 @@ void trampProcess(uint32_t currentTimeUs) if (trampStatus == TRAMP_STATUS_BAD_DEVICE) return; - trampReceive(currentTimeUs); + // trampReceive(currentTimeUs); if (trampStatus == TRAMP_STATUS_OFFLINE) { if (cmp32(currentTimeUs, lastQueryRTimeUs) > 1000 * 1000) { From 465870cb0c37168011f953df2f73a638da570267 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 13 Jan 2017 03:26:56 +0900 Subject: [PATCH 11/26] Delay 1sec between freq and power --- src/main/io/vtx_tramp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 0d1664f3a2..e0842da374 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -372,6 +372,8 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) // XXX Does Tramp handles back-to-back commands properly!? // Test without back-to-back commands. + delay(1000); + trampSetRFPower(trampCmsPowerTable[trampCmsPower]); trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; From 4696c4ad4589f9f3c6ff7e0af2dca2ff651f2d0b Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 13 Jan 2017 12:41:06 +0900 Subject: [PATCH 12/26] Debugging trampReceive: array boundary checking Removed frame timeout also. --- src/main/io/vtx_tramp.c | 74 ++++++++++++++++++++++++++++------------- 1 file changed, 51 insertions(+), 23 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index e0842da374..ea7f3b21a4 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -34,6 +34,7 @@ #include "io/serial.h" #include "drivers/serial.h" #include "drivers/vtx_var.h" +#include "drivers/system.h" #include "io/vtx_tramp.h" #include "io/vtx_common.h" @@ -199,24 +200,14 @@ typedef enum { static trampReceiveState_e trampReceiveState = S_WAIT_LEN; static int trampReceivePos = 0; -static uint32_t trampFrameStartUs = 0; - -// Frame timeout. An actual frame (16B) takes only 16.6 msec, -// but a frame arrival may span two scheduling intervals (200msec * 2). -// Effectively same as waiting for a next trampProcess() to run. - -#define TRAMP_FRAME_TIMO_US (200 * 1000) void trampReceive(uint32_t currentTimeUs) { + UNUSED(currentTimeUs); + if (!trampSerialPort) return; - if ((trampReceiveState != S_WAIT_LEN) && cmp32(currentTimeUs, trampFrameStartUs > TRAMP_FRAME_TIMO_US)) { - trampReceiveState = S_WAIT_LEN; - trampReceivePos = 0; - } - while (serialRxBytesWaiting(trampSerialPort)) { uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; @@ -225,7 +216,6 @@ void trampReceive(uint32_t currentTimeUs) case S_WAIT_LEN: if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; - trampFrameStartUs = currentTimeUs; } else { trampReceivePos = 0; } @@ -251,6 +241,15 @@ void trampReceive(uint32_t currentTimeUs) trampReceivePos = 0; } break; + + default: + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; + } + + // Debugging... + if (trampReceivePos >= 16) { + debug[0]++; } } } @@ -321,13 +320,36 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; -static long trampCmsUpdateFreqRef(displayPort_t *pDisp, const void *self) +static void trampCmsUpdateFreqRef(void) +{ + if (trampCmsBand > 0 && trampCmsChan > 0) + trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; +} + +static long trampCmsConfigBand(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (trampCmsBand > 0 && trampCmsChan > 0) - trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; + if (trampCmsBand == 0) + // Bounce back + trampCmsBand = 1; + else + trampCmsUpdateFreqRef(); + + return 0; +} + +static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (trampCmsChan == 0) + // Bounce back + trampCmsChan = 1; + else + trampCmsUpdateFreqRef(); return 0; } @@ -347,7 +369,7 @@ static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NUL static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; static const char * const trampCmsPitmodeNames[] = { - "OFF", "ON " + "---", "OFF", "ON " }; static OSD_TAB_t trampCmsEntPitmode = { &trampCmsPitmode, 2, trampCmsPitmodeNames, NULL }; @@ -357,7 +379,12 @@ static long trampCmsSetPitmode(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - trampSetPitmode(trampCmsPitmode); + if (trampCmsPitmode == 0) { + // Bouce back + trampCmsPitmode = 1; + } else { + trampSetPitmode(trampCmsPitmode - 1); + } return 0; } @@ -369,10 +396,11 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) trampSetBandChan(trampCmsBand, trampCmsChan); - // XXX Does Tramp handles back-to-back commands properly!? - // Test without back-to-back commands. + // Tramp doesn't handle back-to-back commands properly. + // Insert some delay here. Will do no harm provided CMS is activated + // only when disarmed. - delay(1000); + delay(100); trampSetRFPower(trampCmsPowerTable[trampCmsPower]); @@ -403,8 +431,8 @@ static OSD_Entry trampMenuEntries[] = { "", OME_Label, NULL, trampCmsStatusString, DYNAMIC }, { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, - { "BAND", OME_TAB, trampCmsUpdateFreqRef, &trampCmsEntBand, 0 }, - { "CHAN", OME_TAB, trampCmsUpdateFreqRef, &trampCmsEntChan, 0 }, + { "BAND", OME_TAB, trampCmsConfigBand, &trampCmsEntBand, 0 }, + { "CHAN", OME_TAB, trampCmsConfigChan, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, { "TEMP", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, From 0818999530bad5c9df7360f8b1e1e2c3f6f55668 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 13 Jan 2017 14:09:22 +0100 Subject: [PATCH 13/26] tramp: use async changes + small fixes --- src/main/io/vtx_tramp.c | 126 ++++++++++++++++++++++++++++++++-------- 1 file changed, 102 insertions(+), 24 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index ea7f3b21a4..3cc708ceb4 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -46,7 +46,9 @@ static uint8_t trampRespBuffer[16]; typedef enum { TRAMP_STATUS_BAD_DEVICE = -1, TRAMP_STATUS_OFFLINE = 0, - TRAMP_STATUS_ONLINE + TRAMP_STATUS_ONLINE, + TRAMP_STATUS_SET_FREQ_PW, + TRAMP_STATUS_CHECK_FREQ_PW } trampStatus_e; trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; @@ -62,8 +64,12 @@ uint16_t trampCurPower = 0; // Actual transmitting power uint16_t trampCurConfigPower = 0; // Configured transmitting power int16_t trampCurTemp = 0; +uint32_t trampConfFreq = 0; +uint16_t trampConfPower = 0; + #ifdef CMS -void trampCmsUpdateStatusString(void); // Forward +static void trampCmsUpdateStatusString(void); // Forward +static void trampCmsUpdateBandChan(void); // Forward #endif static void trampWriteBuf(uint8_t *buf) @@ -96,20 +102,40 @@ void trampCmdU16(uint8_t cmd, uint16_t param) } void trampSetFreq(uint16_t freq) +{ + trampConfFreq = freq; +} + +void trampSendFreq(uint16_t freq) { trampCmdU16('F', freq); } void trampSetBandChan(uint8_t band, uint8_t chan) { - trampCmdU16('F', vtx58FreqTable[band - 1][chan - 1]); + trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); } void trampSetRFPower(uint16_t level) +{ + trampConfPower = level; +} + +void trampSendRFPower(uint16_t level) { trampCmdU16('P', level); } +// return false if error +bool trampCommitChanges() +{ + if(trampStatus != TRAMP_STATUS_ONLINE) + return false; + + trampStatus = TRAMP_STATUS_SET_FREQ_PW; + return true; +} + void trampSetPitmode(uint8_t onoff) { trampCmdU16('I', onoff ? 0x0100 : 0); @@ -169,10 +195,21 @@ void trampHandleResponse(void) break; case 'v': - trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); - trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); - vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + { + uint32_t oldCurFreq = trampCurFreq; + trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); + vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + +#ifdef CMS + if(!oldCurFreq && trampCurFreq) + trampCmsUpdateBandChan(); +#endif + + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + trampStatus = TRAMP_STATUS_SET_FREQ_PW; + } break; case 's': @@ -234,7 +271,7 @@ void trampReceive(uint32_t currentTimeUs) if (trampReceivePos == 16) { uint8_t cksum = trampChecksum(trampRespBuffer); if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { - // trampHandleResponse(); + trampHandleResponse(); } trampReceiveState = S_WAIT_LEN; @@ -257,22 +294,60 @@ void trampReceive(uint32_t currentTimeUs) void trampProcess(uint32_t currentTimeUs) { static uint32_t lastQueryRTimeUs = 0; + static uint32_t lastQueryVTimeUs = 0; if (trampStatus == TRAMP_STATUS_BAD_DEVICE) return; - // trampReceive(currentTimeUs); + trampReceive(currentTimeUs); if (trampStatus == TRAMP_STATUS_OFFLINE) { - if (cmp32(currentTimeUs, lastQueryRTimeUs) > 1000 * 1000) { + if (cmp32(currentTimeUs, lastQueryRTimeUs) > 1000 * 1000) { // 1s trampQueryR(); lastQueryRTimeUs = currentTimeUs; } - } else if (trampReceiveState == S_WAIT_LEN) { - if (trampPendingQuery) - trampQuery(trampPendingQuery); - else - trampQueryV(); + } else { + if (trampStatus == TRAMP_STATUS_SET_FREQ_PW) { + + bool done = false; + if (trampConfFreq != trampCurFreq) { + trampSendFreq(trampConfFreq); + } + else if (trampConfPower != trampCurPower) { + trampSendRFPower(trampConfPower); + } + + if(!done) { + trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; + + // delay next status query by 200ms + lastQueryVTimeUs = currentTimeUs + 200 * 1000; + } + else { + // everything has been done, let's return to original state + trampStatus = TRAMP_STATUS_ONLINE; + } + } + else if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { + if (cmp32(currentTimeUs, lastQueryVTimeUs) > 50 * 1000) { + trampQueryV(); + lastQueryVTimeUs = currentTimeUs; + } + } + else if (trampStatus == TRAMP_STATUS_ONLINE) { + + if (cmp32(currentTimeUs, lastQueryVTimeUs) > 1000 * 1000) { + trampQueryV(); + lastQueryVTimeUs = currentTimeUs; + } + } + + /* if (trampReceiveState == S_WAIT_LEN) { */ + /* if (trampPendingQuery) */ + /* trampQuery(trampPendingQuery); */ + /* else */ + /* trampQueryV(); */ + /* } */ } #ifdef CMS @@ -289,7 +364,7 @@ char trampCmsStatusString[31] = "- -- ---- ----"; // m bc ffff tppp // 01234567890123 -void trampCmsUpdateStatusString(void) +static void trampCmsUpdateStatusString(void) { trampCmsStatusString[0] = '*'; trampCmsStatusString[1] = ' '; @@ -326,6 +401,13 @@ static void trampCmsUpdateFreqRef(void) trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; } +static void trampCmsUpdateBandChan() +{ + if(trampCurBand > 0) trampCmsBand = trampCurBand; + if(trampCurChan > 0) trampCmsChan = trampCurChan; + trampCmsUpdateFreqRef(); +} + static long trampCmsConfigBand(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); @@ -355,7 +437,7 @@ static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) } static const char * const trampCmsPowerNames[] = { - "25", "100", "200", "400", "600" + "25 ", "100", "200", "400", "600" }; static const uint16_t trampCmsPowerTable[] = { @@ -395,15 +477,11 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(self); trampSetBandChan(trampCmsBand, trampCmsChan); - - // Tramp doesn't handle back-to-back commands properly. - // Insert some delay here. Will do no harm provided CMS is activated - // only when disarmed. - - delay(100); - trampSetRFPower(trampCmsPowerTable[trampCmsPower]); + // TODO: error handling + trampCommitChanges(); + trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; return MENU_CHAIN_BACK; From 868256617d7f4256e4d353c72e85067684e254d9 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 13 Jan 2017 15:26:26 +0100 Subject: [PATCH 14/26] fixed pit-mode Beware that the so called pit-mode is more some kind of a power switch. You might need to stick the receiver antenna against the transmitter to receive the very weak signal. --- src/main/io/vtx_tramp.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 3cc708ceb4..16560ea121 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -63,6 +63,7 @@ uint8_t trampCurChan = 0; uint16_t trampCurPower = 0; // Actual transmitting power uint16_t trampCurConfigPower = 0; // Configured transmitting power int16_t trampCurTemp = 0; +uint8_t trampCurPitmode = 0; uint32_t trampConfFreq = 0; uint16_t trampConfPower = 0; @@ -138,7 +139,7 @@ bool trampCommitChanges() void trampSetPitmode(uint8_t onoff) { - trampCmdU16('I', onoff ? 0x0100 : 0); + trampCmdU16('I', onoff ? 0 : 1); } static uint8_t trampPendingQuery = 0; // XXX Assume no code/resp == 0 @@ -201,6 +202,7 @@ void trampHandleResponse(void) trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + trampCurPitmode = trampRespBuffer[7]; #ifdef CMS if(!oldCurFreq && trampCurFreq) @@ -406,6 +408,7 @@ static void trampCmsUpdateBandChan() if(trampCurBand > 0) trampCmsBand = trampCurBand; if(trampCurChan > 0) trampCmsChan = trampCurChan; trampCmsUpdateFreqRef(); + trampCmsPitmode = trampCurPitmode + 1; } static long trampCmsConfigBand(displayPort_t *pDisp, const void *self) From 632ab29e48b68e6668e2d75c9543675dc7fce1b4 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 13 Jan 2017 18:21:28 +0100 Subject: [PATCH 15/26] tramp: fixed initial cms menu settings --- src/main/io/vtx_tramp.c | 65 +++++++++++++++++++++++------------------ 1 file changed, 37 insertions(+), 28 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 16560ea121..a9201262b2 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -70,7 +70,6 @@ uint16_t trampConfPower = 0; #ifdef CMS static void trampCmsUpdateStatusString(void); // Forward -static void trampCmsUpdateBandChan(void); // Forward #endif static void trampWriteBuf(uint8_t *buf) @@ -197,18 +196,12 @@ void trampHandleResponse(void) case 'v': { - uint32_t oldCurFreq = trampCurFreq; trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); trampCurPitmode = trampRespBuffer[7]; -#ifdef CMS - if(!oldCurFreq && trampCurFreq) - trampCmsUpdateBandChan(); -#endif - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) trampStatus = TRAMP_STATUS_SET_FREQ_PW; } @@ -397,20 +390,24 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; +static const char * const trampCmsPowerNames[] = { + "25 ", "100", "200", "400", "600" +}; + +static const uint16_t trampCmsPowerTable[] = { + 25, 100, 200, 400, 600 +}; + +static uint8_t trampCmsPower = 0; + +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NULL }; + static void trampCmsUpdateFreqRef(void) { if (trampCmsBand > 0 && trampCmsChan > 0) trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; } -static void trampCmsUpdateBandChan() -{ - if(trampCurBand > 0) trampCmsBand = trampCurBand; - if(trampCurChan > 0) trampCmsChan = trampCurChan; - trampCmsUpdateFreqRef(); - trampCmsPitmode = trampCurPitmode + 1; -} - static long trampCmsConfigBand(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); @@ -439,18 +436,6 @@ static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) return 0; } -static const char * const trampCmsPowerNames[] = { - "25 ", "100", "200", "400", "600" -}; - -static const uint16_t trampCmsPowerTable[] = { - 25, 100, 200, 400, 600 -}; - -static uint8_t trampCmsPower = 0; - -static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NULL }; - static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; static const char * const trampCmsPitmodeNames[] = { @@ -490,6 +475,30 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) return MENU_CHAIN_BACK; } +static void trampCmsInitSettings() +{ + if(trampCurBand > 0) trampCmsBand = trampCurBand; + if(trampCurChan > 0) trampCmsChan = trampCurChan; + + trampCmsUpdateFreqRef(); + trampCmsPitmode = trampCurPitmode + 1; + + if (trampCurConfigPower > 0) { + for (uint8_t i = 0; i < sizeof(trampCmsPowerTable); i++) { + if (trampCurConfigPower <= trampCmsPowerTable[i]) { + trampCmsPower = i; + break; + } + } + } +} + +static long trampCmsOnEnter() +{ + trampCmsInitSettings(); + return 0; +} + static OSD_Entry trampCmsMenuCommenceEntries[] = { { "CONFIRM", OME_Label, NULL, NULL, 0 }, { "YES", OME_Funcall, trampCmsCommence, NULL, 0 }, @@ -527,7 +536,7 @@ static OSD_Entry trampMenuEntries[] = CMS_Menu cmsx_menuVtxTramp = { .GUARD_text = "XVTXTR", .GUARD_type = OME_MENU, - .onEnter = NULL, + .onEnter = trampCmsOnEnter, .onExit = NULL, .onGlobalExit = NULL, .entries = trampMenuEntries, From 0fe30c06e23741d2bc01e8b9d42738a0a29334b0 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 00:07:47 +0100 Subject: [PATCH 16/26] some cleanup --- src/main/io/vtx_tramp.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index a9201262b2..fac2db9fc0 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -165,7 +165,6 @@ void trampQueryS(void) } #define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) -//#define TRAMP_SERIAL_OPTIONS (0) // For debugging with tramp emulator bool trampInit() { @@ -467,10 +466,9 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) trampSetBandChan(trampCmsBand, trampCmsChan); trampSetRFPower(trampCmsPowerTable[trampCmsPower]); - // TODO: error handling + // If it fails, the user should retry later trampCommitChanges(); - trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; return MENU_CHAIN_BACK; } @@ -527,7 +525,6 @@ static OSD_Entry trampMenuEntries[] = { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, { "TEMP", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, - //{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } From 58a778c797172da0cf924b4a6f182ffe370d481f Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 00:08:39 +0100 Subject: [PATCH 17/26] tramp: removed temperature support until scale is defined --- src/main/io/vtx_tramp.c | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index fac2db9fc0..d6e848715f 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -62,7 +62,6 @@ uint8_t trampCurBand = 0; uint8_t trampCurChan = 0; uint16_t trampCurPower = 0; // Actual transmitting power uint16_t trampCurConfigPower = 0; // Configured transmitting power -int16_t trampCurTemp = 0; uint8_t trampCurPitmode = 0; uint32_t trampConfFreq = 0; @@ -205,10 +204,6 @@ void trampHandleResponse(void) trampStatus = TRAMP_STATUS_SET_FREQ_PW; } break; - - case 's': - trampCurTemp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); - break; } if (trampPendingQuery == respCode) @@ -435,8 +430,6 @@ static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) return 0; } -static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; - static const char * const trampCmsPitmodeNames[] = { "---", "OFF", "ON " }; @@ -523,7 +516,6 @@ static OSD_Entry trampMenuEntries[] = { "CHAN", OME_TAB, trampCmsConfigChan, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, - { "TEMP", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, From dddd09ae8720492713782dafd616678b0e81ba79 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 11:59:44 +0100 Subject: [PATCH 18/26] tramp: moved common definitions to vtx_common --- Makefile | 5 ++--- src/main/drivers/vtx_var.c | 26 -------------------------- src/main/drivers/vtx_var.h | 8 -------- src/main/io/vtx_common.c | 27 +++++++++++++++++++++++++-- src/main/io/vtx_common.h | 7 +++++++ src/main/io/vtx_smartaudio.c | 3 +-- src/main/io/vtx_tramp.c | 5 ++--- 7 files changed, 37 insertions(+), 44 deletions(-) delete mode 100644 src/main/drivers/vtx_var.c delete mode 100644 src/main/drivers/vtx_var.h diff --git a/Makefile b/Makefile index c04f22854c..1a7a352ab0 100644 --- a/Makefile +++ b/Makefile @@ -610,7 +610,6 @@ HIGHEND_SRC = \ telemetry/mavlink.c \ telemetry/ibus.c \ sensors/esc_sensor.c \ - drivers/vtx_var.c \ io/vtx_common.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c @@ -718,8 +717,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ cms/cms_menu_misc.c \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ - io/vtx_smartaudio.c - # io/vtx_tramp.c # makes binary bigger!? + io/vtx_smartaudio.c \ + io/vtx_tramp.c endif #F3 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) diff --git a/src/main/drivers/vtx_var.c b/src/main/drivers/vtx_var.c deleted file mode 100644 index f541b4a6a5..0000000000 --- a/src/main/drivers/vtx_var.c +++ /dev/null @@ -1,26 +0,0 @@ -#include -#include "vtx_var.h" - -const uint16_t vtx58FreqTable[5][8] = -{ - { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A - { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B - { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E - { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark - { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand -}; - -const char * const vtx58BandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -const char vtx58BandLetter[] = "-ABEFR"; - -const char * const vtx58ChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; diff --git a/src/main/drivers/vtx_var.h b/src/main/drivers/vtx_var.h deleted file mode 100644 index c8e7cdf2dc..0000000000 --- a/src/main/drivers/vtx_var.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -extern const uint16_t vtx58FreqTable[5][8]; -extern const char * const vtx58BandNames[]; -extern const char * const vtx58ChanNames[]; -extern const char vtx58BandLetter[]; diff --git a/src/main/io/vtx_common.c b/src/main/io/vtx_common.c index 62b9c00f0f..5f89be8dd3 100644 --- a/src/main/io/vtx_common.c +++ b/src/main/io/vtx_common.c @@ -25,8 +25,31 @@ #include "platform.h" #include "build/debug.h" -#if defined(VTX_TRAMP) -#include "drivers/vtx_var.h" +#if defined(VTX_CONTROL) + +const uint16_t vtx58FreqTable[5][8] = +{ + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A + { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B + { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E + { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark + { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand +}; + +const char * const vtx58BandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +const char vtx58BandLetter[] = "-ABEFR"; + +const char * const vtx58ChannelNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan) { diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h index 8c260279b3..c4f5c4e47f 100644 --- a/src/main/io/vtx_common.h +++ b/src/main/io/vtx_common.h @@ -1,3 +1,10 @@ #pragma once +#include + +extern const uint16_t vtx58FreqTable[5][8]; +extern const char * const vtx58BandNames[]; +extern const char * const vtx58ChannelNames[]; +extern const char vtx58BandLetter[]; + bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 8ece43894b..e5118a4f00 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -33,7 +33,6 @@ #include "common/utils.h" #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/vtx_var.h" #include "io/serial.h" #include "io/vtx_smartaudio.h" @@ -985,7 +984,7 @@ static CMS_Menu saCmsMenuStats = { static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL }; -static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChanNames[0], NULL }; +static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChannelNames[0], NULL }; static const char * const saCmsPowerNames[] = { "---", diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index d6e848715f..72fc990e4d 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -33,7 +33,6 @@ #include "io/serial.h" #include "drivers/serial.h" -#include "drivers/vtx_var.h" #include "drivers/system.h" #include "io/vtx_tramp.h" #include "io/vtx_common.h" @@ -358,7 +357,7 @@ static void trampCmsUpdateStatusString(void) trampCmsStatusString[0] = '*'; trampCmsStatusString[1] = ' '; trampCmsStatusString[2] = vtx58BandLetter[trampCurBand]; - trampCmsStatusString[3] = vtx58ChanNames[trampCurChan][0]; + trampCmsStatusString[3] = vtx58ChannelNames[trampCurChan][0]; trampCmsStatusString[4] = ' '; if (trampCurFreq) @@ -380,7 +379,7 @@ uint16_t trampCmsFreqRef; static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames, NULL }; -static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChanNames, NULL }; +static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }; static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; From c74e4f9a5c5b04f49345530d611d0b0d370ef294 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 12:20:39 +0100 Subject: [PATCH 19/26] fixed missing include --- src/main/io/vtx_smartaudio.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index e5118a4f00..a10db12bf7 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -35,6 +35,7 @@ #include "drivers/serial.h" #include "io/serial.h" #include "io/vtx_smartaudio.h" +#include "io/vtx_common.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" From 56c8fb0183405d03b596b7680dc4a478568f2ca0 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 12:21:51 +0100 Subject: [PATCH 20/26] re-enabled VTX_SMARTAUDIO --- src/main/target/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/common.h b/src/main/target/common.h index 848570746d..b818ddf574 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -104,7 +104,7 @@ #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define VTX_CONTROL -//#define VTX_SMARTAUDIO +#define VTX_SMARTAUDIO #define VTX_TRAMP #define USE_SENSOR_NAMES #else From c29f8a79ed84602cfc904d76270e92f69313cd6a Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 12:22:18 +0100 Subject: [PATCH 21/26] use separate serial functions for SmartAudio and Tramp. --- src/main/io/serial.h | 3 ++- src/main/io/vtx_smartaudio.c | 4 ++-- src/main/io/vtx_tramp.c | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index b8371cc322..5a68a18041 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -37,8 +37,9 @@ typedef enum { FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 FUNCTION_ESC_SENSOR = (1 << 10), // 1024 - FUNCTION_VTX_CONTROL = (1 << 11), // 2048 + FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096 + FUNCTION_VTX_TRAMP = (1 << 13), // 8192 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index a10db12bf7..c3b93077a1 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -652,9 +652,9 @@ bool smartAudioInit() } #endif - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); } if (!smartAudioSerialPort) { diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 72fc990e4d..f57a10c8b5 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -166,10 +166,10 @@ void trampQueryS(void) bool trampInit() { - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); } if (!trampSerialPort) { From 07659576b11f4952ef1ad8aecc635bb08c9ca16b Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 14:11:49 +0100 Subject: [PATCH 22/26] fixed pre-processor directives --- src/main/io/vtx_common.h | 4 ++++ src/main/io/vtx_smartaudio.c | 2 +- src/main/io/vtx_tramp.c | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h index c4f5c4e47f..cd593048b6 100644 --- a/src/main/io/vtx_common.h +++ b/src/main/io/vtx_common.h @@ -2,9 +2,13 @@ #include +#if defined(VTX_CONTROL) + extern const uint16_t vtx58FreqTable[5][8]; extern const char * const vtx58BandNames[]; extern const char * const vtx58ChannelNames[]; extern const char vtx58BandLetter[]; bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan); + +#endif diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index c3b93077a1..19b9cad91c 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -23,7 +23,7 @@ #include "platform.h" -#ifdef VTX_SMARTAUDIO +#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) #include "cms/cms.h" #include "cms/cms_types.h" diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f57a10c8b5..c52bb76fa4 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -24,7 +24,7 @@ #include "platform.h" -#ifdef VTX_TRAMP +#if defined(VTX_TRAMP) && defined(VTX_CONTROL) #include "build/debug.h" From 99cff4a87521884d3222fd44964a27f0931e4168 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 18:34:14 +0100 Subject: [PATCH 23/26] some clean-up work (@ledvinap comments) --- src/main/io/vtx_tramp.c | 292 ++++++++++++++++++++++------------------ 1 file changed, 158 insertions(+), 134 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index c52bb76fa4..42f8bd5b34 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -139,11 +139,121 @@ void trampSetPitmode(uint8_t onoff) trampCmdU16('I', onoff ? 0 : 1); } -static uint8_t trampPendingQuery = 0; // XXX Assume no code/resp == 0 +// returns completed response code +char trampHandleResponse(void) +{ + uint8_t respCode = trampRespBuffer[1]; + + switch (respCode) { + case 'r': + { + uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if(min_freq != 0) { + trampRFFreqMin = min_freq; + trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); + return 'r'; + } + + // throw bytes echoed from tx to rx in bidirectional mode away + } + break; + + case 'v': + { + uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if(freq != 0) { + trampCurFreq = freq; + trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampCurPitmode = trampRespBuffer[7]; + trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); + vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + return 'v'; + } + + // throw bytes echoed from tx to rx in bidirectional mode away + } + break; + } + + return 0; +} + +typedef enum { + S_WAIT_LEN = 0, // Waiting for a packet len + S_WAIT_CODE, // Waiting for a response code + S_DATA, // Waiting for rest of the packet. +} trampReceiveState_e; + +static trampReceiveState_e trampReceiveState = S_WAIT_LEN; +static int trampReceivePos = 0; + +static void trampResetReceiver() +{ + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; +} + +static bool trampIsValidResponseCode(uint8_t code) +{ + if (code == 'r' || code == 'v' || code == 's') + return true; + else + return false; +} + +// returns completed response code or 0 +static char trampReceive(uint32_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!trampSerialPort) + return 0; + + while (serialRxBytesWaiting(trampSerialPort)) { + uint8_t c = serialRead(trampSerialPort); + trampRespBuffer[trampReceivePos++] = c; + + switch(trampReceiveState) { + case S_WAIT_LEN: + if (c == 0x0F) { + trampReceiveState = S_WAIT_CODE; + } else { + trampReceivePos = 0; + } + break; + + case S_WAIT_CODE: + if (trampIsValidResponseCode(c)) { + trampReceiveState = S_DATA; + } else { + trampResetReceiver(); + } + break; + + case S_DATA: + if (trampReceivePos == 16) { + uint8_t cksum = trampChecksum(trampRespBuffer); + + trampResetReceiver(); + + if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { + return trampHandleResponse(); + } + } + break; + + default: + trampResetReceiver(); + } + } + + return 0; +} void trampQuery(uint8_t cmd) { - trampPendingQuery = cmd; + trampResetReceiver(); trampCmdU16(cmd, 0); } @@ -179,163 +289,77 @@ bool trampInit() return true; } -void trampHandleResponse(void) -{ - uint8_t respCode = trampRespBuffer[1]; - - switch (respCode) { - case 'r': - trampRFFreqMin = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); - trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); - trampStatus = TRAMP_STATUS_ONLINE; - break; - - case 'v': - { - trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); - trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); - vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); - trampCurPitmode = trampRespBuffer[7]; - - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) - trampStatus = TRAMP_STATUS_SET_FREQ_PW; - } - break; - } - - if (trampPendingQuery == respCode) - trampPendingQuery = 0; -} - -static bool trampIsValidResponseCode(uint8_t code) -{ - if (code == 'r' || code == 'v' || code == 's') - return true; - else - return false; -} - -typedef enum { - S_WAIT_LEN = 0, // Waiting for a packet len - S_WAIT_CODE, // Waiting for a response code - S_DATA, // Waiting for rest of the packet. -} trampReceiveState_e; - -static trampReceiveState_e trampReceiveState = S_WAIT_LEN; -static int trampReceivePos = 0; - -void trampReceive(uint32_t currentTimeUs) -{ - UNUSED(currentTimeUs); - - if (!trampSerialPort) - return; - - while (serialRxBytesWaiting(trampSerialPort)) { - uint8_t c = serialRead(trampSerialPort); - trampRespBuffer[trampReceivePos++] = c; - - switch(trampReceiveState) { - case S_WAIT_LEN: - if (c == 0x0F) { - trampReceiveState = S_WAIT_CODE; - } else { - trampReceivePos = 0; - } - break; - - case S_WAIT_CODE: - if (trampIsValidResponseCode(c)) { - trampReceiveState = S_DATA; - } else { - trampReceiveState = S_WAIT_LEN; - trampReceivePos = 0; - } - break; - - case S_DATA: - if (trampReceivePos == 16) { - uint8_t cksum = trampChecksum(trampRespBuffer); - if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { - trampHandleResponse(); - } - - trampReceiveState = S_WAIT_LEN; - trampReceivePos = 0; - } - break; - - default: - trampReceiveState = S_WAIT_LEN; - trampReceivePos = 0; - } - - // Debugging... - if (trampReceivePos >= 16) { - debug[0]++; - } - } -} - void trampProcess(uint32_t currentTimeUs) { - static uint32_t lastQueryRTimeUs = 0; - static uint32_t lastQueryVTimeUs = 0; + static uint32_t lastQueryTimeUs = 0; if (trampStatus == TRAMP_STATUS_BAD_DEVICE) return; - trampReceive(currentTimeUs); + char replyCode = trampReceive(currentTimeUs); - if (trampStatus == TRAMP_STATUS_OFFLINE) { - if (cmp32(currentTimeUs, lastQueryRTimeUs) > 1000 * 1000) { // 1s - trampQueryR(); - lastQueryRTimeUs = currentTimeUs; + switch(replyCode) { + case 'r': + if (trampStatus <= TRAMP_STATUS_OFFLINE) + trampStatus = TRAMP_STATUS_ONLINE; + break; + + case 'v': + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + trampStatus = TRAMP_STATUS_SET_FREQ_PW; + break; + } + + switch(trampStatus) { + + case TRAMP_STATUS_OFFLINE: + case TRAMP_STATUS_ONLINE: + + if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s + + if (trampStatus == TRAMP_STATUS_OFFLINE) + trampQueryR(); + else + trampQueryV(); + + lastQueryTimeUs = currentTimeUs; } - } else { - if (trampStatus == TRAMP_STATUS_SET_FREQ_PW) { + break; - bool done = false; + case TRAMP_STATUS_SET_FREQ_PW: + { + bool done = true; if (trampConfFreq != trampCurFreq) { trampSendFreq(trampConfFreq); + done = false; } - else if (trampConfPower != trampCurPower) { + else if (trampConfPower != trampCurConfigPower) { trampSendRFPower(trampConfPower); + done = false; } if(!done) { trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; - // delay next status query by 200ms - lastQueryVTimeUs = currentTimeUs + 200 * 1000; + // delay next status query by 300ms + lastQueryTimeUs = currentTimeUs + 300 * 1000; } else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; } } - else if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { - if (cmp32(currentTimeUs, lastQueryVTimeUs) > 50 * 1000) { - trampQueryV(); - lastQueryVTimeUs = currentTimeUs; - } - } - else if (trampStatus == TRAMP_STATUS_ONLINE) { - - if (cmp32(currentTimeUs, lastQueryVTimeUs) > 1000 * 1000) { - trampQueryV(); - lastQueryVTimeUs = currentTimeUs; - } - } + break; - /* if (trampReceiveState == S_WAIT_LEN) { */ - /* if (trampPendingQuery) */ - /* trampQuery(trampPendingQuery); */ - /* else */ - /* trampQueryV(); */ - /* } */ + case TRAMP_STATUS_CHECK_FREQ_PW: + if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) { + trampQueryV(); + lastQueryTimeUs = currentTimeUs; + } + break; + + default: + break; } #ifdef CMS From af72b761b534dc8a624c21a0d75c16ee1fd009c8 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 23:18:38 +0100 Subject: [PATCH 24/26] fixed pre-processor directives (continued) --- src/main/cms/cms_menu_builtin.c | 2 ++ src/main/fc/fc_init.c | 4 ++++ src/main/io/vtx_tramp.h | 4 ++++ src/main/target/common_post.h | 1 - 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 783864ffe1..1827655268 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -97,12 +97,14 @@ static OSD_Entry menuFeaturesEntries[] = #if defined(VTX) || defined(USE_RTC6705) {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 +#if defined(VTX_CONTROL) #if defined(VTX_SMARTAUDIO) {"VTX SA", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, #endif #if defined(VTX_TRAMP) {"VTX TR", OME_Submenu, cmsMenuChange, &cmsx_menuVtxTramp, 0}, #endif +#endif // VTX_CONTROL #ifdef LED_STRIP {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, #endif // LED_STRIP diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f86682bb66..cbe0fdb3ec 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -524,6 +524,8 @@ void init(void) baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif +#ifdef VTX_CONTROL + #ifdef VTX_SMARTAUDIO smartAudioInit(); #endif @@ -532,6 +534,8 @@ void init(void) trampInit(); #endif +#endif // VTX_CONTROL + // start all timers // TODO - not implemented yet timerStart(); diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 0d08d0d293..1864cf0a97 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -1,5 +1,7 @@ #pragma once +#if defined(VTX_TRAMP) && defined(VTX_CONTROL) + bool trampInit(); void trampProcess(uint32_t currentTimeUs); @@ -8,3 +10,5 @@ void trampProcess(uint32_t currentTimeUs); #include "cms/cms_types.h" extern CMS_Menu cmsx_menuVtxTramp; #endif + +#endif diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 193124c071..d881516c14 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -22,5 +22,4 @@ // Targets with built-in vtx do not need external vtx #if defined(VTX) || defined(USE_RTC6705) # undef VTX_CONTROL -# undef VTX_SMARTAUDIO #endif From 47bbd46c88ea5743a3792aa4246710fcf57b829b Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 15 Jan 2017 23:26:08 +0100 Subject: [PATCH 25/26] added conditional debug (TRAMP_DEBUG) --- src/main/io/vtx_tramp.c | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 42f8bd5b34..f7c25e0942 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -293,11 +293,20 @@ void trampProcess(uint32_t currentTimeUs) { static uint32_t lastQueryTimeUs = 0; +#ifdef TRAMP_DEBUG + static uint16_t debugFreqReqCounter = 0; + static uint16_t debugPowReqCounter = 0; +#endif + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) return; char replyCode = trampReceive(currentTimeUs); +#ifdef TRAMP_DEBUG + debug[0] = trampStatus; +#endif + switch(replyCode) { case 'r': if (trampStatus <= TRAMP_STATUS_OFFLINE) @@ -331,10 +340,16 @@ void trampProcess(uint32_t currentTimeUs) bool done = true; if (trampConfFreq != trampCurFreq) { trampSendFreq(trampConfFreq); +#ifdef TRAMP_DEBUG + debugFreqReqCounter++; +#endif done = false; } else if (trampConfPower != trampCurConfigPower) { trampSendRFPower(trampConfPower); +#ifdef TRAMP_DEBUG + debugPowReqCounter++; +#endif done = false; } @@ -362,6 +377,12 @@ void trampProcess(uint32_t currentTimeUs) break; } +#ifdef TRAMP_DEBUG + debug[1] = debugFreqReqCounter; + debug[2] = debugPowReqCounter; + debug[3] = 0; +#endif + #ifdef CMS trampCmsUpdateStatusString(); #endif From 9681929f98cfac2b3433b605bcf788ef8bacf0a2 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Mon, 16 Jan 2017 11:13:53 +0100 Subject: [PATCH 26/26] tramp: re-enabled temperature support Temps have been measured right on the STM32F031G6 located on the surface of the Tramp and seem to be quite accurate. Above 104C, it starts to trickle the power down. --- src/main/io/vtx_tramp.c | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f7c25e0942..4e4716836a 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -61,6 +61,7 @@ uint8_t trampCurBand = 0; uint8_t trampCurChan = 0; uint16_t trampCurPower = 0; // Actual transmitting power uint16_t trampCurConfigPower = 0; // Configured transmitting power +int16_t trampCurTemp = 0; uint8_t trampCurPitmode = 0; uint32_t trampConfFreq = 0; @@ -174,6 +175,16 @@ char trampHandleResponse(void) // throw bytes echoed from tx to rx in bidirectional mode away } break; + + case 's': + { + uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + if(temp != 0) { + trampCurTemp = temp; + return 's'; + } + } + break; } return 0; @@ -323,13 +334,17 @@ void trampProcess(uint32_t currentTimeUs) case TRAMP_STATUS_OFFLINE: case TRAMP_STATUS_ONLINE: - if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s if (trampStatus == TRAMP_STATUS_OFFLINE) trampQueryR(); - else - trampQueryV(); + else { + static unsigned int cnt = 0; + if(((cnt++) & 1) == 0) + trampQueryV(); + else + trampQueryS(); + } lastQueryTimeUs = currentTimeUs; } @@ -474,6 +489,8 @@ static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) return 0; } +static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; + static const char * const trampCmsPitmodeNames[] = { "---", "OFF", "ON " }; @@ -560,6 +577,7 @@ static OSD_Entry trampMenuEntries[] = { "CHAN", OME_TAB, trampCmsConfigChan, &trampCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, + { "T(C)", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, { "BACK", OME_Back, NULL, NULL, 0 },