Fix WS2812 led definition
[inav.git] / src / main / io / vtx_ffpv24g.c
blob87c29418dacfe1ac9108605419b420568bbe904f
1 /*
2 * This file is part of INAV Project.
4 * This Source Code Form is subject to the terms of the Mozilla Public
5 * License, v. 2.0. If a copy of the MPL was not distributed with this file,
6 * You can obtain one at http://mozilla.org/MPL/2.0/.
8 * Alternatively, the contents of this file may be used under the terms
9 * of the GNU General Public License Version 3, as described below:
11 * This file is free software: you may copy, redistribute and/or modify
12 * it under the terms of the GNU General Public License as published by the
13 * Free Software Foundation, either version 3 of the License, or (at your
14 * option) any later version.
16 * This file is distributed in the hope that it will be useful, but
17 * WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19 * Public License for more details.
21 * You should have received a copy of the GNU General Public License
22 * along with this program. If not, see http://www.gnu.org/licenses/.
25 #include "platform.h"
27 #include <stdbool.h>
28 #include <stdint.h>
29 #include <ctype.h>
30 #include <string.h>
32 #if defined(USE_VTX_FFPV) && defined(USE_VTX_CONTROL)
34 #include "build/debug.h"
36 #include "drivers/time.h"
37 #include "drivers/vtx_common.h"
39 #include "common/maths.h"
40 #include "common/utils.h"
42 #include "scheduler/protothreads.h"
44 #include "io/vtx.h"
45 #include "io/vtx_ffpv24g.h"
46 #include "io/vtx_control.h"
47 #include "io/vtx_string.h"
48 #include "io/serial.h"
51 #define VTX_FFPV_CMD_TIMEOUT_MS 250
52 #define VTX_FFPV_HEARTBEAT_MS 1000
54 #define VTX_FFPV_MIN_BAND (1)
55 #define VTX_FFPV_MAX_BAND (VTX_FFPV_MIN_BAND + VTX_FFPV_BAND_COUNT - 1)
56 #define VTX_FFPV_MIN_CHANNEL (1)
57 #define VTX_FFPV_MAX_CHANNEL (VTX_FFPV_MIN_CHANNEL + VTX_FFPV_CHANNEL_COUNT -1)
59 #define VTX_UPDATE_REQ_NONE 0x00
60 #define VTX_UPDATE_REQ_FREQUENCY 0x01
61 #define VTX_UPDATE_REQ_POWER 0x02
63 typedef struct __attribute__((__packed__)) ffpvPacket_s {
64 uint8_t header;
65 uint8_t cmd;
66 uint8_t data[12];
67 uint8_t checksum;
68 uint8_t footer;
69 } ffpvPacket_t;
71 typedef struct {
72 bool ready;
73 int protoTimeouts;
74 unsigned updateReqMask;
76 // VTX capabilities
77 struct {
78 unsigned freqMin;
79 unsigned freqMax;
80 unsigned powerMin;
81 unsigned powerMax;
82 } capabilities;
84 // Requested VTX state
85 struct {
86 int band;
87 int channel;
88 unsigned freq;
89 unsigned power;
90 unsigned powerIndex;
91 } request;
93 // Actual VTX state
94 struct {
95 unsigned freq;
96 unsigned power;
97 } state;
99 // Comms flags and state
100 ffpvPacket_t sendPkt;
101 ffpvPacket_t recvPkt;
102 unsigned recvPtr;
103 bool pktReceived;
104 } vtxProtoState_t;
106 /*****************************************************************************/
107 const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
108 "-----",
109 "A 2.4",
110 "B 2.4",
113 const char * ffpvBandLetters = "-AB";
115 const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT] =
117 { 2410, 2430, 2450, 2470, 2370, 2390, 2490, 2510 }, // FFPV 2.4 A
118 { 2414, 2432, 2450, 2468, 2411, 2433, 2453, 2473 }, // FFPV 2.4 A
121 const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1] = {
122 "-", "1", "2", "3", "4", "5", "6", "7", "8",
125 const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1] = {
126 "---", "25 ", "200", "500", "800"
129 const unsigned ffpvPowerTable[VTX_FFPV_POWER_COUNT] = {
130 25, 200, 500, 800
134 /*******************************************************************************/
135 static serialPort_t * vtxSerialPort = NULL;
136 static vtxProtoState_t vtxState;
138 static uint8_t vtxCalcChecksum(ffpvPacket_t * pkt)
140 uint8_t sum = pkt->cmd;
141 for (int i = 0; i < 12; i++) {
142 sum += pkt->data[i];
144 return sum;
147 static bool vtxProtoRecv(void)
149 // Return success instantly if packet is already awaiting processing
150 if (vtxState.pktReceived) {
151 return true;
154 uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
155 while (serialRxBytesWaiting(vtxSerialPort) && !vtxState.pktReceived) {
156 const uint8_t c = serialRead(vtxSerialPort);
158 if (vtxState.recvPtr == 0) {
159 // Wait for sync byte
160 if (c == 0x0F) {
161 bufPtr[vtxState.recvPtr++] = c;
164 else {
165 // Sync byte ok - wait for full packet
166 if (vtxState.recvPtr < sizeof(vtxState.recvPkt)) {
167 bufPtr[vtxState.recvPtr++] = c;
170 // Received full packet - do some processing
171 if (vtxState.recvPtr == sizeof(vtxState.recvPkt)) {
172 // Full packet received - validate packet, make sure it's the one we expect
173 const bool pktValid = (vtxState.recvPkt.header == 0x0F && vtxState.recvPkt.cmd == vtxState.sendPkt.cmd && vtxState.recvPkt.footer == 0x00 && vtxState.recvPkt.checksum == vtxCalcChecksum(&vtxState.recvPkt));
174 if (!pktValid) {
175 // Reset the receiver state - keep waiting
176 vtxState.pktReceived = false;
177 vtxState.recvPtr = 0;
179 // Make sure it's not the echo one (half-duplex serial might receive it's own data)
180 else if (memcmp(&vtxState.recvPkt.data, &vtxState.sendPkt.data, sizeof(vtxState.recvPkt.data)) == 0) {
181 vtxState.pktReceived = false;
182 vtxState.recvPtr = 0;
184 // Valid receive packet
185 else {
186 vtxState.pktReceived = true;
187 return true;
193 return false;
196 static void vtxProtoSend(uint8_t cmd, const uint8_t * data)
198 // Craft and send FPV packet
199 vtxState.sendPkt.header = 0x0F;
200 vtxState.sendPkt.cmd = cmd;
202 if (data) {
203 memcpy(vtxState.sendPkt.data, data, sizeof(vtxState.sendPkt.data));
205 else {
206 ZERO_FARRAY(vtxState.sendPkt.data);
209 vtxState.sendPkt.checksum = vtxCalcChecksum(&vtxState.sendPkt);
210 vtxState.sendPkt.footer = 0x00;
212 // Send data
213 serialWriteBuf(vtxSerialPort, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
215 // Reset cmd response state
216 vtxState.pktReceived = false;
217 vtxState.recvPtr = 0;
220 static void vtxProtoSend_SetFreqency(unsigned freq)
222 uint8_t data[12] = {0};
223 data[0] = freq & 0xFF;
224 data[1] = (freq >> 8) & 0xFF;
225 vtxProtoSend(0x46, data);
228 static void vtxProtoSend_SetPower(unsigned power)
230 uint8_t data[12] = {0};
231 data[0] = power & 0xFF;
232 data[1] = (power >> 8) & 0xFF;
233 vtxProtoSend(0x50, data);
236 STATIC_PROTOTHREAD(impl_VtxProtocolThread)
238 ptBegin(impl_VtxProtocolThread);
240 // 0: Detect VTX. Dwell here infinitely until we get a valid response from VTX
241 vtxState.ready = false;
242 while(!vtxState.ready) {
243 // Send capabilities request and wait
244 vtxProtoSend(0x72, NULL);
245 ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS);
247 // Check if we got a valid response
248 if (vtxState.pktReceived) {
249 vtxState.capabilities.freqMin = vtxState.recvPkt.data[0] | (vtxState.recvPkt.data[1] << 8);
250 vtxState.capabilities.freqMax = vtxState.recvPkt.data[2] | (vtxState.recvPkt.data[3] << 8);
251 vtxState.capabilities.powerMin = 0;
252 vtxState.capabilities.powerMax = vtxState.recvPkt.data[4] | (vtxState.recvPkt.data[5] << 8);
253 vtxState.ready = true;
257 // 1 : Periodically poll VTX for current channel and power, send updates
258 vtxState.protoTimeouts = 0;
259 vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
261 while(vtxState.ready) {
262 // Wait for request for update or time to check liveness
263 ptWaitTimeout(vtxState.updateReqMask != VTX_UPDATE_REQ_NONE, VTX_FFPV_HEARTBEAT_MS);
265 if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
266 if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) {
267 vtxProtoSend_SetFreqency(vtxState.request.freq);
268 vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY;
269 ptDelayMs(VTX_FFPV_CMD_TIMEOUT_MS);
271 else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
272 vtxProtoSend_SetPower(vtxState.request.power);
273 vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
275 else {
276 // Unsupported request - reset
277 vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
280 else {
281 // Periodic check for VTX liveness
282 vtxProtoSend(0x76, NULL);
283 ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS);
285 if (vtxState.pktReceived) {
286 // Got a valid state from VTX
287 vtxState.state.freq = (uint16_t)vtxState.recvPkt.data[0] | ((uint16_t)vtxState.recvPkt.data[1] << 8);
288 vtxState.state.power = (uint16_t)vtxState.recvPkt.data[2] | ((uint16_t)vtxState.recvPkt.data[3] << 8);
289 vtxState.protoTimeouts = 0;
291 // Check if VTX state matches VTX request
292 if (vtxState.state.freq != vtxState.request.freq) {
293 vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
296 if (vtxState.state.power != vtxState.request.power) {
297 vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
300 else {
301 vtxState.protoTimeouts++;
305 // Sanity check. If we got more than 3 protocol erros
306 if (vtxState.protoTimeouts >= 3) {
307 // Reset ready flag - thread will terminate and restart
308 vtxState.ready = false;
312 ptEnd(0);
316 static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
318 // Glue function betwen VTX VTable and actual driver protothread
319 UNUSED(vtxDevice);
320 UNUSED(currentTimeUs);
322 impl_VtxProtocolThread();
324 // If thread stopped - vtx comms failed - restart thread and re-init VTX comms
325 if (ptIsStopped(ptGetHandle(impl_VtxProtocolThread))) {
326 ptRestart(ptGetHandle(impl_VtxProtocolThread));
330 static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
332 UNUSED(vtxDevice);
333 return VTXDEV_FFPV;
336 static bool impl_IsReady(const vtxDevice_t *vtxDevice)
338 return vtxDevice != NULL && vtxSerialPort != NULL && vtxState.ready;
341 static bool impl_DevSetFreq(uint16_t freq)
343 if (!vtxState.ready || freq < vtxState.capabilities.freqMin || freq > vtxState.capabilities.freqMax) {
344 return false;
347 vtxState.request.freq = freq;
348 vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
350 return true;
353 static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
355 UNUSED(vtxDevice);
357 // Validate band and channel
358 if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) {
359 return;
362 if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) {
363 // Keep track of band/channel data
364 vtxState.request.band = band;
365 vtxState.request.channel = channel;
369 static void ffpvSetRFPowerByIndex(uint16_t index)
371 // Validate index
372 if (index < 1 || index > VTX_FFPV_POWER_COUNT) {
373 return;
376 const unsigned power = ffpvPowerTable[index - 1];
377 if (!vtxState.ready || power < vtxState.capabilities.powerMin || power > vtxState.capabilities.powerMax) {
378 return;
381 vtxState.request.power = power;
382 vtxState.request.powerIndex = index;
383 vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
386 static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
388 UNUSED(vtxDevice);
389 ffpvSetRFPowerByIndex(index);
392 static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
394 // TODO: Not implemented
395 UNUSED(vtxDevice);
396 UNUSED(onoff);
399 static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
401 if (!impl_IsReady(vtxDevice)) {
402 return false;
405 // if in user-freq mode then report band as zero
406 *pBand = vtxState.request.band;
407 *pChannel = vtxState.request.channel;
408 return true;
411 static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
413 if (!impl_IsReady(vtxDevice)) {
414 return false;
417 *pIndex = vtxState.request.powerIndex;
419 return true;
422 static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
424 if (!impl_IsReady(vtxDevice)) {
425 return false;
428 // TODO: Not inplemented
429 *pOnOff = 0;
430 return true;
433 static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
435 if (!impl_IsReady(vtxDevice)) {
436 return false;
439 *pFreq = vtxState.request.freq;
440 return true;
443 static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
445 if (!impl_IsReady(vtxDevice)) {
446 return false;
449 *pIndex = vtxState.request.powerIndex;
450 *pPowerMw = vtxState.request.power;
451 return true;
454 static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
456 if (!impl_IsReady(vtxDevice)) {
457 return false;
460 pOsdInfo->band = vtxState.request.band;
461 pOsdInfo->channel = vtxState.request.channel;
462 pOsdInfo->frequency = vtxState.request.freq;
463 pOsdInfo->powerIndex = vtxState.request.powerIndex;
464 pOsdInfo->powerMilliwatt = vtxState.request.power;
465 pOsdInfo->bandName = ffpvBandNames[vtxState.request.band];
466 pOsdInfo->bandLetter = ffpvBandLetters[vtxState.request.band];
467 pOsdInfo->channelName = ffpvChannelNames[vtxState.request.channel];
468 pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
469 return true;
472 /*****************************************************************************/
473 static const vtxVTable_t impl_vtxVTable = {
474 .process = impl_Process,
475 .getDeviceType = impl_GetDeviceType,
476 .isReady = impl_IsReady,
477 .setBandAndChannel = impl_SetBandAndChannel,
478 .setPowerByIndex = impl_SetPowerByIndex,
479 .setPitMode = impl_SetPitMode,
480 .getBandAndChannel = impl_GetBandAndChannel,
481 .getPowerIndex = impl_GetPowerIndex,
482 .getPitMode = impl_GetPitMode,
483 .getFrequency = impl_GetFreq,
484 .getPower = impl_GetPower,
485 .getOsdInfo = impl_GetOsdInfo,
488 static vtxDevice_t impl_vtxDevice = {
489 .vTable = &impl_vtxVTable,
490 .capability.bandCount = VTX_FFPV_BAND_COUNT,
491 .capability.channelCount = VTX_FFPV_CHANNEL_COUNT,
492 .capability.powerCount = VTX_FFPV_POWER_COUNT,
493 .capability.bandNames = (char **)ffpvBandNames,
494 .capability.channelNames = (char **)ffpvChannelNames,
495 .capability.powerNames = (char **)ffpvPowerNames,
498 bool vtxFuriousFPVInit(void)
500 serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_VTX_FFPV);
502 if (portConfig) {
503 portOptions_t portOptions = 0;
504 portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
505 vtxSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_FFPV, NULL, NULL, 9600, MODE_RXTX, portOptions);
508 if (!vtxSerialPort) {
509 return false;
512 vtxCommonSetDevice(&impl_vtxDevice);
514 ptRestart(ptGetHandle(impl_VtxProtocolThread));
516 return true;
519 #endif