From 6550d738db1efe3bafc45548a832a7bae48fcca1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 12 Jan 2017 16:53:22 +0900 Subject: [PATCH] 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