src/main/io/vtx_ffpv24g.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   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/.
   7  *
   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:
  10  *
  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.
  15  *
  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.
  20  *
  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/.
  23  */
  24
  25 #include "platform.h"
  26
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29 #include <ctype.h>
  30 #include <string.h>
  31
  32 #if defined(USE_VTX_FFPV) && defined(USE_VTX_CONTROL)
  33
  34 #include "build/debug.h"
  35
  36 #include "drivers/time.h"
  37 #include "drivers/vtx_common.h"
  38
  39 #include "common/maths.h"
  40 #include "common/utils.h"
  41
  42 #include "scheduler/protothreads.h"
  43
  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"
  49
  50
  51 #define VTX_FFPV_CMD_TIMEOUT_MS     250
  52 #define VTX_FFPV_HEARTBEAT_MS       1000
  53
  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)
  58
  59 #define VTX_UPDATE_REQ_NONE         0x00
  60 #define VTX_UPDATE_REQ_FREQUENCY    0x01
  61 #define VTX_UPDATE_REQ_POWER        0x02
  62
  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;
  70
  71 typedef struct {
  72     bool ready;
  73     int protoTimeouts;
  74     unsigned updateReqMask;
  75
  76     // VTX capabilities
  77     struct {
  78         unsigned freqMin;
  79         unsigned freqMax;
  80         unsigned powerMin;
  81         unsigned powerMax;
  82     } capabilities;
  83
  84     // Requested VTX state
  85     struct {
  86         int      band;
  87         int      channel;
  88         unsigned freq;
  89         unsigned power;
  90         unsigned powerIndex;
  91     } request;
  92
  93     // Actual VTX state
  94     struct {
  95         unsigned freq;
  96         unsigned power;
  97     } state;
  98
  99     // Comms flags and state
 100     ffpvPacket_t sendPkt;
 101     ffpvPacket_t recvPkt;
 102     unsigned     recvPtr;
 103     bool         pktReceived;
 104 } vtxProtoState_t;
 105
 106 /*****************************************************************************/
 107 const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
 108     "-----",
 109     "A 2.4",
 110     "B 2.4",
 111 };
 112
 113 const char * ffpvBandLetters = "-AB";
 114
 115 const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT] =
 116 {
 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
 119 };
 120
 121 const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1] = {
 122     "-", "1", "2", "3", "4", "5", "6", "7", "8",
 123 };
 124
 125 const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1] = {
 126     "---", "25 ", "200", "500", "800"
 127 };
 128
 129 const unsigned ffpvPowerTable[VTX_FFPV_POWER_COUNT] = {
 130     25, 200, 500, 800
 131 };
 132
 133
 134 /*******************************************************************************/
 135 static serialPort_t * vtxSerialPort = NULL;
 136 static vtxProtoState_t vtxState;
 137
 138 static uint8_t vtxCalcChecksum(ffpvPacket_t * pkt)
 139 {
 140     uint8_t sum = pkt->cmd;
 141     for (int i = 0; i < 12; i++) {
 142         sum += pkt->data[i];
 143     }
 144     return sum;
 145 }
 146
 147 static bool vtxProtoRecv(void)
 148 {
 149     // Return success instantly if packet is already awaiting processing
 150     if (vtxState.pktReceived) {
 151         return true;
 152     }
 153
 154     uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
 155     while (serialRxBytesWaiting(vtxSerialPort) && !vtxState.pktReceived) {
 156         const uint8_t c = serialRead(vtxSerialPort);
 157
 158         if (vtxState.recvPtr == 0) {
 159             // Wait for sync byte
 160             if (c == 0x0F) {
 161                 bufPtr[vtxState.recvPtr++] = c;
 162             }
 163         }
 164         else {
 165             // Sync byte ok - wait for full packet
 166             if (vtxState.recvPtr < sizeof(vtxState.recvPkt)) {
 167                 bufPtr[vtxState.recvPtr++] = c;
 168             }
 169
 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;
 178                 }
 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;
 183                 }
 184                 // Valid receive packet
 185                 else {
 186                     vtxState.pktReceived = true;
 187                     return true;
 188                 }
 189             }
 190         }
 191     }
 192
 193     return false;
 194 }
 195
 196 static void vtxProtoSend(uint8_t cmd, const uint8_t * data)
 197 {
 198     // Craft and send FPV packet
 199     vtxState.sendPkt.header = 0x0F;
 200     vtxState.sendPkt.cmd = cmd;
 201
 202     if (data) {
 203         memcpy(vtxState.sendPkt.data, data, sizeof(vtxState.sendPkt.data));
 204     }
 205     else {
 206         ZERO_FARRAY(vtxState.sendPkt.data);
 207     }
 208
 209     vtxState.sendPkt.checksum = vtxCalcChecksum(&vtxState.sendPkt);
 210     vtxState.sendPkt.footer = 0x00;
 211
 212     // Send data
 213     serialWriteBuf(vtxSerialPort, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
 214
 215     // Reset cmd response state
 216     vtxState.pktReceived = false;
 217     vtxState.recvPtr = 0;
 218 }
 219
 220 static void vtxProtoSend_SetFreqency(unsigned freq)
 221 {
 222     uint8_t data[12] = {0};
 223     data[0] = freq & 0xFF;
 224     data[1] = (freq >> 8) & 0xFF;
 225     vtxProtoSend(0x46, data);
 226 }
 227
 228 static void vtxProtoSend_SetPower(unsigned power)
 229 {
 230     uint8_t data[12] = {0};
 231     data[0] = power & 0xFF;
 232     data[1] = (power >> 8) & 0xFF;
 233     vtxProtoSend(0x50, data);
 234 }
 235
 236 STATIC_PROTOTHREAD(impl_VtxProtocolThread)
 237 {
 238     ptBegin(impl_VtxProtocolThread);
 239
 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);
 246
 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;
 254         }
 255     }
 256
 257     // 1 : Periodically poll VTX for current channel and power, send updates
 258     vtxState.protoTimeouts = 0;
 259     vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
 260
 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);
 264
 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);
 270             }
 271             else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
 272                 vtxProtoSend_SetPower(vtxState.request.power);
 273                 vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
 274             }
 275             else {
 276                 // Unsupported request - reset
 277                 vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
 278             }
 279         }
 280         else {
 281             // Periodic check for VTX liveness
 282             vtxProtoSend(0x76, NULL);
 283             ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS);
 284
 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;
 290
 291                 // Check if VTX state matches VTX request
 292                 if (vtxState.state.freq != vtxState.request.freq) {
 293                     vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 294                 }
 295
 296                 if (vtxState.state.power != vtxState.request.power) {
 297                     vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 298                 }
 299             }
 300             else {
 301                 vtxState.protoTimeouts++;
 302             }
 303         }
 304
 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;
 309         }
 310     }
 311
 312     ptEnd(0);
 313 }
 314
 315
 316 static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 317 {
 318     // Glue function betwen VTX VTable and actual driver protothread
 319     UNUSED(vtxDevice);
 320     UNUSED(currentTimeUs);
 321
 322     impl_VtxProtocolThread();
 323
 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));
 327     }
 328 }
 329
 330 static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
 331 {
 332     UNUSED(vtxDevice);
 333     return VTXDEV_FFPV;
 334 }
 335
 336 static bool impl_IsReady(const vtxDevice_t *vtxDevice)
 337 {
 338     return vtxDevice != NULL && vtxSerialPort != NULL && vtxState.ready;
 339 }
 340
 341 static bool impl_DevSetFreq(uint16_t freq)
 342 {
 343     if (!vtxState.ready || freq < vtxState.capabilities.freqMin || freq > vtxState.capabilities.freqMax) {
 344         return false;
 345     }
 346
 347     vtxState.request.freq = freq;
 348     vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 349
 350     return true;
 351 }
 352
 353 static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
 354 {
 355     UNUSED(vtxDevice);
 356
 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;
 360     }
 361
 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;
 366     }
 367 }
 368
 369 static void ffpvSetRFPowerByIndex(uint16_t index)
 370 {
 371     // Validate index
 372     if (index < 1 || index > VTX_FFPV_POWER_COUNT) {
 373         return;
 374     }
 375
 376     const unsigned power = ffpvPowerTable[index - 1];
 377     if (!vtxState.ready || power < vtxState.capabilities.powerMin || power > vtxState.capabilities.powerMax) {
 378         return;
 379     }
 380
 381     vtxState.request.power = power;
 382     vtxState.request.powerIndex = index;
 383     vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 384 }
 385
 386 static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
 387 {
 388     UNUSED(vtxDevice);
 389     ffpvSetRFPowerByIndex(index);
 390 }
 391
 392 static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 393 {
 394     // TODO: Not implemented
 395     UNUSED(vtxDevice);
 396     UNUSED(onoff);
 397 }
 398
 399 static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 400 {
 401     if (!impl_IsReady(vtxDevice)) {
 402         return false;
 403     }
 404
 405     // if in user-freq mode then report band as zero
 406     *pBand = vtxState.request.band;
 407     *pChannel = vtxState.request.channel;
 408     return true;
 409 }
 410
 411 static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 412 {
 413     if (!impl_IsReady(vtxDevice)) {
 414         return false;
 415     }
 416
 417     *pIndex = vtxState.request.powerIndex;
 418
 419     return true;
 420 }
 421
 422 static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 423 {
 424     if (!impl_IsReady(vtxDevice)) {
 425         return false;
 426     }
 427
 428     // TODO: Not inplemented
 429     *pOnOff = 0;
 430     return true;
 431 }
 432
 433 static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 434 {
 435     if (!impl_IsReady(vtxDevice)) {
 436         return false;
 437     }
 438
 439     *pFreq = vtxState.request.freq;
 440     return true;
 441 }
 442
 443 static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 444 {
 445     if (!impl_IsReady(vtxDevice)) {
 446         return false;
 447     }
 448
 449     *pIndex = vtxState.request.powerIndex;
 450     *pPowerMw = vtxState.request.power;
 451     return true;
 452 }
 453
 454 static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 455 {
 456     if (!impl_IsReady(vtxDevice)) {
 457         return false;
 458     }
 459
 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;
 470 }
 471
 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,
 486 };
 487
 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,
 496 };
 497
 498 bool vtxFuriousFPVInit(void)
 499 {
 500     serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_VTX_FFPV);
 501
 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);
 506     }
 507
 508     if (!vtxSerialPort) {
 509         return false;
 510     }
 511
 512     vtxCommonSetDevice(&impl_vtxDevice);
 513
 514     ptRestart(ptGetHandle(impl_VtxProtocolThread));
 515
 516     return true;
 517 }
 518
 519 #endif