2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
18 /* Created by jflyper */
27 #if defined(VTX_TRAMP) && defined(VTX_CONTROL)
29 #include "build/debug.h"
31 #include "common/utils.h"
32 #include "common/printf.h"
34 #include "io/serial.h"
35 #include "drivers/serial.h"
36 #include "drivers/system.h"
37 #include "drivers/vtx_common.h"
38 #include "io/vtx_tramp.h"
39 #include "io/vtx_string.h"
41 #define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
43 #if defined(CMS) || defined(VTX_COMMON)
44 static const uint16_t trampPowerTable
[] = {
45 25, 100, 200, 400, 600
48 static const char * const trampPowerNames
[] = {
49 "---", "25 ", "100", "200", "400", "600"
53 #if defined(VTX_COMMON)
54 static vtxDevice_t vtxTramp
= {
56 .capability
.bandCount
= 5,
57 .capability
.channelCount
= 8,
58 .capability
.powerCount
= sizeof(trampPowerTable
),
59 .bandNames
= (char **)vtx58BandNames
,
60 .channelNames
= (char **)vtx58ChannelNames
,
61 .powerNames
= (char **)trampPowerNames
,
65 static serialPort_t
*trampSerialPort
= NULL
;
67 static uint8_t trampReqBuffer
[16];
68 static uint8_t trampRespBuffer
[16];
71 TRAMP_STATUS_BAD_DEVICE
= -1,
72 TRAMP_STATUS_OFFLINE
= 0,
74 TRAMP_STATUS_SET_FREQ_PW
,
75 TRAMP_STATUS_CHECK_FREQ_PW
78 trampStatus_e trampStatus
= TRAMP_STATUS_OFFLINE
;
80 uint32_t trampRFFreqMin
;
81 uint32_t trampRFFreqMax
;
82 uint32_t trampRFPowerMax
;
84 uint32_t trampCurFreq
= 0;
85 uint8_t trampBand
= 0;
86 uint8_t trampChannel
= 0;
87 uint16_t trampPower
= 0; // Actual transmitting power
88 uint16_t trampConfiguredPower
= 0; // Configured transmitting power
89 int16_t trampTemperature
= 0;
90 uint8_t trampPitMode
= 0;
92 // Maximum number of requests sent to try a config change
93 #define TRAMP_MAX_RETRIES 2
95 uint32_t trampConfFreq
= 0;
96 uint8_t trampFreqRetries
= 0;
98 uint16_t trampConfPower
= 0;
99 uint8_t trampPowerRetries
= 0;
102 static void trampCmsUpdateStatusString(void); // Forward
105 static void trampWriteBuf(uint8_t *buf
)
107 serialWriteBuf(trampSerialPort
, buf
, 16);
110 static uint8_t trampChecksum(uint8_t *trampBuf
)
114 for (int i
= 1 ; i
< 14 ; i
++)
115 cksum
+= trampBuf
[i
];
120 void trampCmdU16(uint8_t cmd
, uint16_t param
)
122 if (!trampSerialPort
)
125 memset(trampReqBuffer
, 0, ARRAYLEN(trampReqBuffer
));
126 trampReqBuffer
[0] = 15;
127 trampReqBuffer
[1] = cmd
;
128 trampReqBuffer
[2] = param
& 0xff;
129 trampReqBuffer
[3] = (param
>> 8) & 0xff;
130 trampReqBuffer
[14] = trampChecksum(trampReqBuffer
);
131 trampWriteBuf(trampReqBuffer
);
134 void trampSetFreq(uint16_t freq
)
136 trampConfFreq
= freq
;
137 if(trampConfFreq
!= trampCurFreq
)
138 trampFreqRetries
= TRAMP_MAX_RETRIES
;
141 void trampSendFreq(uint16_t freq
)
143 trampCmdU16('F', freq
);
146 void trampSetBandAndChannel(uint8_t band
, uint8_t channel
)
148 trampSetFreq(vtx58frequencyTable
[band
- 1][channel
- 1]);
151 void trampSetRFPower(uint16_t level
)
153 trampConfPower
= level
;
154 if(trampConfPower
!= trampPower
)
155 trampPowerRetries
= TRAMP_MAX_RETRIES
;
158 void trampSendRFPower(uint16_t level
)
160 trampCmdU16('P', level
);
163 // return false if error
164 bool trampCommitChanges()
166 if(trampStatus
!= TRAMP_STATUS_ONLINE
)
169 trampStatus
= TRAMP_STATUS_SET_FREQ_PW
;
173 void trampSetPitMode(uint8_t onoff
)
175 trampCmdU16('I', onoff
? 0 : 1);
178 // returns completed response code
179 char trampHandleResponse(void)
181 uint8_t respCode
= trampRespBuffer
[1];
186 uint16_t min_freq
= trampRespBuffer
[2]|(trampRespBuffer
[3] << 8);
188 trampRFFreqMin
= min_freq
;
189 trampRFFreqMax
= trampRespBuffer
[4]|(trampRespBuffer
[5] << 8);
190 trampRFPowerMax
= trampRespBuffer
[6]|(trampRespBuffer
[7] << 8);
194 // throw bytes echoed from tx to rx in bidirectional mode away
200 uint16_t freq
= trampRespBuffer
[2]|(trampRespBuffer
[3] << 8);
203 trampConfiguredPower
= trampRespBuffer
[4]|(trampRespBuffer
[5] << 8);
204 trampPitMode
= trampRespBuffer
[7];
205 trampPower
= trampRespBuffer
[8]|(trampRespBuffer
[9] << 8);
206 vtx58_Freq2Bandchan(trampCurFreq
, &trampBand
, &trampChannel
);
208 if(trampConfFreq
== 0) trampConfFreq
= trampCurFreq
;
209 if(trampConfPower
== 0) trampConfPower
= trampPower
;
213 // throw bytes echoed from tx to rx in bidirectional mode away
219 uint16_t temp
= (int16_t)(trampRespBuffer
[6]|(trampRespBuffer
[7] << 8));
221 trampTemperature
= temp
;
232 S_WAIT_LEN
= 0, // Waiting for a packet len
233 S_WAIT_CODE
, // Waiting for a response code
234 S_DATA
, // Waiting for rest of the packet.
235 } trampReceiveState_e
;
237 static trampReceiveState_e trampReceiveState
= S_WAIT_LEN
;
238 static int trampReceivePos
= 0;
240 static void trampResetReceiver()
242 trampReceiveState
= S_WAIT_LEN
;
246 static bool trampIsValidResponseCode(uint8_t code
)
248 if (code
== 'r' || code
== 'v' || code
== 's')
254 // returns completed response code or 0
255 static char trampReceive(uint32_t currentTimeUs
)
257 UNUSED(currentTimeUs
);
259 if (!trampSerialPort
)
262 while (serialRxBytesWaiting(trampSerialPort
)) {
263 uint8_t c
= serialRead(trampSerialPort
);
264 trampRespBuffer
[trampReceivePos
++] = c
;
266 switch(trampReceiveState
) {
269 trampReceiveState
= S_WAIT_CODE
;
276 if (trampIsValidResponseCode(c
)) {
277 trampReceiveState
= S_DATA
;
279 trampResetReceiver();
284 if (trampReceivePos
== 16) {
285 uint8_t cksum
= trampChecksum(trampRespBuffer
);
287 trampResetReceiver();
289 if ((trampRespBuffer
[14] == cksum
) && (trampRespBuffer
[15] == 0)) {
290 return trampHandleResponse();
296 trampResetReceiver();
303 void trampQuery(uint8_t cmd
)
305 trampResetReceiver();
309 void trampQueryR(void)
314 void trampQueryV(void)
319 void trampQueryS(void)
324 void vtxTrampProcess(uint32_t currentTimeUs
)
326 static uint32_t lastQueryTimeUs
= 0;
329 static uint16_t debugFreqReqCounter
= 0;
330 static uint16_t debugPowReqCounter
= 0;
333 if (trampStatus
== TRAMP_STATUS_BAD_DEVICE
)
336 char replyCode
= trampReceive(currentTimeUs
);
339 debug
[0] = trampStatus
;
344 if (trampStatus
<= TRAMP_STATUS_OFFLINE
)
345 trampStatus
= TRAMP_STATUS_ONLINE
;
349 if (trampStatus
== TRAMP_STATUS_CHECK_FREQ_PW
)
350 trampStatus
= TRAMP_STATUS_SET_FREQ_PW
;
354 switch(trampStatus
) {
356 case TRAMP_STATUS_OFFLINE
:
357 case TRAMP_STATUS_ONLINE
:
358 if (cmp32(currentTimeUs
, lastQueryTimeUs
) > 1000 * 1000) { // 1s
360 if (trampStatus
== TRAMP_STATUS_OFFLINE
)
363 static unsigned int cnt
= 0;
364 if(((cnt
++) & 1) == 0)
370 lastQueryTimeUs
= currentTimeUs
;
374 case TRAMP_STATUS_SET_FREQ_PW
:
377 if (trampConfFreq
&& trampFreqRetries
&& (trampConfFreq
!= trampCurFreq
)) {
378 trampSendFreq(trampConfFreq
);
381 debugFreqReqCounter
++;
385 else if (trampConfPower
&& trampPowerRetries
&& (trampConfPower
!= trampConfiguredPower
)) {
386 trampSendRFPower(trampConfPower
);
389 debugPowReqCounter
++;
395 trampStatus
= TRAMP_STATUS_CHECK_FREQ_PW
;
397 // delay next status query by 300ms
398 lastQueryTimeUs
= currentTimeUs
+ 300 * 1000;
401 // everything has been done, let's return to original state
402 trampStatus
= TRAMP_STATUS_ONLINE
;
403 // reset configuration value in case it failed (no more retries)
404 trampConfFreq
= trampCurFreq
;
405 trampConfPower
= trampPower
;
406 trampFreqRetries
= trampPowerRetries
= 0;
411 case TRAMP_STATUS_CHECK_FREQ_PW
:
412 if (cmp32(currentTimeUs
, lastQueryTimeUs
) > 200 * 1000) {
414 lastQueryTimeUs
= currentTimeUs
;
423 debug
[1] = debugFreqReqCounter
;
424 debug
[2] = debugPowReqCounter
;
429 trampCmsUpdateStatusString();
435 #include "cms/cms_types.h"
438 char trampCmsStatusString
[31] = "- -- ---- ----";
442 static void trampCmsUpdateStatusString(void)
444 trampCmsStatusString
[0] = '*';
445 trampCmsStatusString
[1] = ' ';
446 trampCmsStatusString
[2] = vtx58BandLetter
[trampBand
];
447 trampCmsStatusString
[3] = vtx58ChannelNames
[trampChannel
][0];
448 trampCmsStatusString
[4] = ' ';
451 tfp_sprintf(&trampCmsStatusString
[5], "%4d", trampCurFreq
);
453 tfp_sprintf(&trampCmsStatusString
[5], "----");
456 tfp_sprintf(&trampCmsStatusString
[9], " %c%3d", (trampPower
== trampConfiguredPower
) ? ' ' : '*', trampPower
);
459 tfp_sprintf(&trampCmsStatusString
[9], " ----");
462 uint8_t trampCmsPitMode
= 0;
463 uint8_t trampCmsBand
= 1;
464 uint8_t trampCmsChan
= 1;
465 uint16_t trampCmsFreqRef
;
467 static OSD_TAB_t trampCmsEntBand
= { &trampCmsBand
, 5, vtx58BandNames
};
469 static OSD_TAB_t trampCmsEntChan
= { &trampCmsChan
, 8, vtx58ChannelNames
};
471 static OSD_UINT16_t trampCmsEntFreqRef
= { &trampCmsFreqRef
, 5600, 5900, 0 };
473 static uint8_t trampCmsPower
= 1;
475 static OSD_TAB_t trampCmsEntPower
= { &trampCmsPower
, 5, trampPowerNames
};
477 static void trampCmsUpdateFreqRef(void)
479 if (trampCmsBand
> 0 && trampCmsChan
> 0)
480 trampCmsFreqRef
= vtx58frequencyTable
[trampCmsBand
- 1][trampCmsChan
- 1];
483 static long trampCmsConfigBand(displayPort_t
*pDisp
, const void *self
)
488 if (trampCmsBand
== 0)
492 trampCmsUpdateFreqRef();
497 static long trampCmsConfigChan(displayPort_t
*pDisp
, const void *self
)
502 if (trampCmsChan
== 0)
506 trampCmsUpdateFreqRef();
511 static long trampCmsConfigPower(displayPort_t
*pDisp
, const void *self
)
516 if (trampCmsPower
== 0)
523 static OSD_INT16_t trampCmsEntTemp
= { &trampTemperature
, -100, 300, 0 };
525 static const char * const trampCmsPitModeNames
[] = {
529 static OSD_TAB_t trampCmsEntPitMode
= { &trampCmsPitMode
, 2, trampCmsPitModeNames
};
531 static long trampCmsSetPitMode(displayPort_t
*pDisp
, const void *self
)
536 if (trampCmsPitMode
== 0) {
540 trampSetPitMode(trampCmsPitMode
- 1);
546 static long trampCmsCommence(displayPort_t
*pDisp
, const void *self
)
551 trampSetBandAndChannel(trampCmsBand
, trampCmsChan
);
552 trampSetRFPower(trampPowerTable
[trampCmsPower
-1]);
554 // If it fails, the user should retry later
555 trampCommitChanges();
558 return MENU_CHAIN_BACK
;
561 static void trampCmsInitSettings()
563 if(trampBand
> 0) trampCmsBand
= trampBand
;
564 if(trampChannel
> 0) trampCmsChan
= trampChannel
;
566 trampCmsUpdateFreqRef();
567 trampCmsPitMode
= trampPitMode
+ 1;
569 if (trampConfiguredPower
> 0) {
570 for (uint8_t i
= 0; i
< sizeof(trampPowerTable
); i
++) {
571 if (trampConfiguredPower
<= trampPowerTable
[i
]) {
572 trampCmsPower
= i
+ 1;
579 static long trampCmsOnEnter()
581 trampCmsInitSettings();
585 static OSD_Entry trampCmsMenuCommenceEntries
[] = {
586 { "CONFIRM", OME_Label
, NULL
, NULL
, 0 },
587 { "YES", OME_Funcall
, trampCmsCommence
, NULL
, 0 },
588 { "BACK", OME_Back
, NULL
, NULL
, 0 },
589 { NULL
, OME_END
, NULL
, NULL
, 0 }
592 static CMS_Menu trampCmsMenuCommence
= {
593 .GUARD_text
= "XVTXTRC",
594 .GUARD_type
= OME_MENU
,
597 .onGlobalExit
= NULL
,
598 .entries
= trampCmsMenuCommenceEntries
,
601 static OSD_Entry trampMenuEntries
[] =
603 { "- TRAMP -", OME_Label
, NULL
, NULL
, 0 },
605 { "", OME_Label
, NULL
, trampCmsStatusString
, DYNAMIC
},
606 { "PIT", OME_TAB
, trampCmsSetPitMode
, &trampCmsEntPitMode
, 0 },
607 { "BAND", OME_TAB
, trampCmsConfigBand
, &trampCmsEntBand
, 0 },
608 { "CHAN", OME_TAB
, trampCmsConfigChan
, &trampCmsEntChan
, 0 },
609 { "(FREQ)", OME_UINT16
, NULL
, &trampCmsEntFreqRef
, DYNAMIC
},
610 { "POWER", OME_TAB
, trampCmsConfigPower
, &trampCmsEntPower
, 0 },
611 { "T(C)", OME_INT16
, NULL
, &trampCmsEntTemp
, DYNAMIC
},
612 { "SET", OME_Submenu
, cmsMenuChange
, &trampCmsMenuCommence
, 0 },
614 { "BACK", OME_Back
, NULL
, NULL
, 0 },
615 { NULL
, OME_END
, NULL
, NULL
, 0 }
618 CMS_Menu cmsx_menuVtxTramp
= {
619 .GUARD_text
= "XVTXTR",
620 .GUARD_type
= OME_MENU
,
621 .onEnter
= trampCmsOnEnter
,
623 .onGlobalExit
= NULL
,
624 .entries
= trampMenuEntries
,
630 // Interface to common VTX API
632 vtxDevType_e
vtxTrampGetDeviceType(void)
637 bool vtxTrampIsReady(void)
639 return trampStatus
> TRAMP_STATUS_OFFLINE
;
642 void vtxTrampSetBandAndChannel(uint8_t band
, uint8_t channel
)
644 if (band
&& channel
) {
645 trampSetBandAndChannel(band
, channel
);
646 trampCommitChanges();
650 void vtxTrampSetPowerByIndex(uint8_t index
)
653 trampSetRFPower(trampPowerTable
[index
- 1]);
654 trampCommitChanges();
658 void vtxTrampSetPitMode(uint8_t onoff
)
660 trampSetPitMode(onoff
);
663 bool vtxTrampGetBandAndChannel(uint8_t *pBand
, uint8_t *pChannel
)
665 if (!vtxTrampIsReady())
669 *pChannel
= trampChannel
;
673 bool vtxTrampGetPowerIndex(uint8_t *pIndex
)
675 if (!vtxTrampIsReady())
678 if (trampConfiguredPower
> 0) {
679 for (uint8_t i
= 0; i
< sizeof(trampPowerTable
); i
++) {
680 if (trampConfiguredPower
<= trampPowerTable
[i
]) {
690 bool vtxTrampGetPitMode(uint8_t *pOnOff
)
692 if (!vtxTrampIsReady())
695 *pOnOff
= trampPitMode
;
699 static vtxVTable_t trampVTable
= {
700 .process
= vtxTrampProcess
,
701 .getDeviceType
= vtxTrampGetDeviceType
,
702 .isReady
= vtxTrampIsReady
,
703 .setBandAndChannel
= vtxTrampSetBandAndChannel
,
704 .setPowerByIndex
= vtxTrampSetPowerByIndex
,
705 .setPitMode
= vtxTrampSetPitMode
,
706 .getBandAndChannel
= vtxTrampGetBandAndChannel
,
707 .getPowerIndex
= vtxTrampGetPowerIndex
,
708 .getPitMode
= vtxTrampGetPitMode
,
713 bool vtxTrampInit(void)
715 serialPortConfig_t
*portConfig
= findSerialPortConfig(FUNCTION_VTX_TRAMP
);
718 trampSerialPort
= openSerialPort(portConfig
->identifier
, FUNCTION_VTX_TRAMP
, NULL
, 9600, MODE_RXTX
, TRAMP_SERIAL_OPTIONS
);
721 if (!trampSerialPort
) {
725 #if defined(VTX_COMMON)
726 vtxTramp
.vTable
= &trampVTable
;
727 vtxCommonRegisterDevice(&vtxTramp
);