rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Some enhancements

This commit is contained in:
jflyper 2017-01-12 16:53:22 +09:00 committed by Raphael Coeffic
parent 0c026a237a
commit 6550d738db
9 changed files with 269 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -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

2
src/main/drivers/vtx_var.c
View File

@ -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",
};

3
src/main/drivers/vtx_var.h
View File

@ -1,5 +1,8 @@
#pragma once
#include <stdint.h>
extern const uint16_t vtx58FreqTable[5][8];
extern const char * const vtx58BandNames[];
extern const char * const vtx58ChanNames[];
extern const char vtx58BandLetter[];

6
src/main/fc/fc_tasks.c
View File

@ -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

52
src/main/io/vtx_common.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/* Created by jflyper */
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#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

2
src/main/io/vtx_common.h
View File

@ -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);

213
src/main/io/vtx_tramp.c
View File

@ -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 },

1
src/main/io/vtx_tramp.h
View File

@ -1,6 +1,7 @@
#pragma once
bool trampInit();
void trampProcess(uint32_t currentTimeUs);
#ifdef CMS
#include "cms/cms.h"

2
src/main/target/common.h
View File

@ -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