src/main/io/vtx_tramp.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /* Created by jflyper */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <ctype.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL)
  31
  32 #include "build/debug.h"
  33
  34 #include "common/maths.h"
  35 #include "common/utils.h"
  36
  37 #include "cms/cms_menu_vtx_tramp.h"
  38
  39 #include "drivers/vtx_common.h"
  40
  41 #include "io/serial.h"
  42 #include "io/vtx_tramp.h"
  43 #include "io/vtx_control.h"
  44 #include "io/vtx.h"
  45 #include "io/vtx_string.h"
  46
  47 #if defined(USE_CMS) || defined(USE_VTX_COMMON)
  48 const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
  49     25, 100, 200, 400, 600
  50 };
  51
  52 const char * trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
  53     "---", "25 ", "100", "200", "400", "600"
  54 };
  55 #endif
  56
  57 #if defined(USE_VTX_COMMON)
  58 static const vtxVTable_t trampVTable; // forward
  59 static vtxDevice_t vtxTramp = {
  60     .vTable = &trampVTable,
  61 };
  62 #endif
  63
  64 static serialPort_t *trampSerialPort = NULL;
  65
  66 static uint8_t trampReqBuffer[16];
  67 static uint8_t trampRespBuffer[16];
  68
  69 typedef enum {
  70     TRAMP_STATUS_BAD_DEVICE = -1,
  71     TRAMP_STATUS_OFFLINE = 0,
  72     TRAMP_STATUS_ONLINE,
  73     TRAMP_STATUS_SET_FREQ_PW,
  74     TRAMP_STATUS_CHECK_FREQ_PW
  75 } trampStatus_e;
  76
  77 trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
  78
  79 uint32_t trampRFFreqMin;
  80 uint32_t trampRFFreqMax;
  81 uint32_t trampRFPowerMax;
  82
  83 bool trampSetByFreqFlag = false;  //false = set via band/channel
  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;
  91
  92 // Maximum number of requests sent to try a config change
  93 #define TRAMP_MAX_RETRIES 2
  94
  95 uint32_t trampConfFreq = 0;
  96 uint8_t  trampFreqRetries = 0;
  97
  98 uint16_t trampConfPower = 0;
  99 uint8_t  trampPowerRetries = 0;
 100
 101 static void trampWriteBuf(uint8_t *buf)
 102 {
 103     serialWriteBuf(trampSerialPort, buf, 16);
 104 }
 105
 106 static uint8_t trampChecksum(uint8_t *trampBuf)
 107 {
 108     uint8_t cksum = 0;
 109
 110     for (int i = 1 ; i < 14 ; i++) {
 111         cksum += trampBuf[i];
 112     }
 113
 114     return cksum;
 115 }
 116
 117 static void trampCmdU16(uint8_t cmd, uint16_t param)
 118 {
 119     if (!trampSerialPort || IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
 120         return;
 121     }
 122
 123     memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
 124     trampReqBuffer[0] = 15;
 125     trampReqBuffer[1] = cmd;
 126     trampReqBuffer[2] = param & 0xff;
 127     trampReqBuffer[3] = (param >> 8) & 0xff;
 128     trampReqBuffer[14] = trampChecksum(trampReqBuffer);
 129     trampWriteBuf(trampReqBuffer);
 130 }
 131
 132 static bool trampValidateFreq(uint16_t freq)
 133 {
 134     return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
 135 }
 136
 137 static void trampDevSetFreq(uint16_t freq)
 138 {
 139     trampConfFreq = freq;
 140     if (trampConfFreq != trampCurFreq) {
 141         trampFreqRetries = TRAMP_MAX_RETRIES;
 142     }
 143 }
 144
 145 void trampSetFreq(uint16_t freq)
 146 {
 147     trampSetByFreqFlag = true;         //set freq via MHz value
 148     trampDevSetFreq(freq);
 149 }
 150
 151 void trampSendFreq(uint16_t freq)
 152 {
 153     trampCmdU16('F', freq);
 154 }
 155
 156 static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel)
 157 {
 158     return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND &&
 159             channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL);
 160 }
 161
 162 static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel)
 163 {
 164     trampDevSetFreq(vtxCommonLookupFrequency(vtxCommonDevice(), band, channel));
 165 }
 166
 167 void trampSetBandAndChannel(uint8_t band, uint8_t channel)
 168 {
 169     trampSetByFreqFlag = false;        //set freq via band/channel
 170     trampDevSetBandAndChannel(band, channel);
 171 }
 172
 173 void trampSetRFPower(uint16_t level)
 174 {
 175     trampConfPower = level;
 176     if (trampConfPower != trampPower) {
 177         trampPowerRetries = TRAMP_MAX_RETRIES;
 178     }
 179 }
 180
 181 void trampSendRFPower(uint16_t level)
 182 {
 183     trampCmdU16('P', level);
 184 }
 185
 186 // return false if error
 187 bool trampCommitChanges(void)
 188 {
 189     if (trampStatus != TRAMP_STATUS_ONLINE) {
 190         return false;
 191     }
 192
 193     trampStatus = TRAMP_STATUS_SET_FREQ_PW;
 194     return true;
 195 }
 196
 197 // return false if index out of range
 198 static bool trampDevSetPowerByIndex(uint8_t index)
 199 {
 200     if (index > 0 && index <= sizeof(trampPowerTable)) {
 201         trampSetRFPower(trampPowerTable[index - 1]);
 202         trampCommitChanges();
 203         return true;
 204     }
 205     return false;
 206 }
 207
 208 void trampSetPitMode(uint8_t onoff)
 209 {
 210     trampCmdU16('I', onoff ? 0 : 1);
 211 }
 212
 213 // returns completed response code
 214 static char trampHandleResponse(void)
 215 {
 216     const uint8_t respCode = trampRespBuffer[1];
 217
 218     switch (respCode) {
 219     case 'r':
 220         {
 221             const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
 222             if (min_freq != 0) {
 223                 trampRFFreqMin = min_freq;
 224                 trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
 225                 trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
 226                 return 'r';
 227             }
 228
 229             // throw bytes echoed from tx to rx in bidirectional mode away
 230         }
 231         break;
 232
 233     case 'v':
 234         {
 235             const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
 236             if (freq != 0) {
 237                 trampCurFreq = freq;
 238                 trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
 239                 trampPitMode = trampRespBuffer[7];
 240                 trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
 241
 242                 // if no band/chan match then make sure set-by-freq mode is flagged
 243                 if (!vtxCommonLookupBandChan(vtxCommonDevice(), trampCurFreq, &trampBand, &trampChannel)) {
 244                     trampSetByFreqFlag = true;
 245                 }
 246
 247                 if (trampConfFreq == 0)  trampConfFreq  = trampCurFreq;
 248                 if (trampConfPower == 0) trampConfPower = trampPower;
 249                 return 'v';
 250             }
 251
 252             // throw bytes echoed from tx to rx in bidirectional mode away
 253         }
 254         break;
 255
 256     case 's':
 257         {
 258             const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
 259             if (temp != 0) {
 260                 trampTemperature = temp;
 261                 return 's';
 262             }
 263         }
 264         break;
 265     }
 266
 267     return 0;
 268 }
 269
 270 typedef enum {
 271     S_WAIT_LEN = 0,   // Waiting for a packet len
 272     S_WAIT_CODE,      // Waiting for a response code
 273     S_DATA,           // Waiting for rest of the packet.
 274 } trampReceiveState_e;
 275
 276 static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
 277 static int trampReceivePos = 0;
 278
 279 static void trampResetReceiver(void)
 280 {
 281     trampReceiveState = S_WAIT_LEN;
 282     trampReceivePos = 0;
 283 }
 284
 285 static bool trampIsValidResponseCode(uint8_t code)
 286 {
 287     if (code == 'r' || code == 'v' || code == 's') {
 288         return true;
 289     } else {
 290         return false;
 291     }
 292 }
 293
 294 // returns completed response code or 0
 295 static char trampReceive(uint32_t currentTimeUs)
 296 {
 297     UNUSED(currentTimeUs);
 298
 299     if (!trampSerialPort) {
 300         return 0;
 301     }
 302
 303     while (serialRxBytesWaiting(trampSerialPort)) {
 304         const uint8_t c = serialRead(trampSerialPort);
 305         trampRespBuffer[trampReceivePos++] = c;
 306
 307         switch (trampReceiveState) {
 308         case S_WAIT_LEN:
 309             if (c == 0x0F) {
 310                 trampReceiveState = S_WAIT_CODE;
 311             } else {
 312                 trampReceivePos = 0;
 313             }
 314             break;
 315
 316         case S_WAIT_CODE:
 317             if (trampIsValidResponseCode(c)) {
 318                 trampReceiveState = S_DATA;
 319             } else {
 320                 trampResetReceiver();
 321             }
 322             break;
 323
 324         case S_DATA:
 325             if (trampReceivePos == 16) {
 326                 uint8_t cksum = trampChecksum(trampRespBuffer);
 327
 328                 trampResetReceiver();
 329
 330                 if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0) && !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
 331                     return trampHandleResponse();
 332                 }
 333             }
 334             break;
 335
 336         default:
 337             trampResetReceiver();
 338             break;
 339         }
 340     }
 341
 342     return 0;
 343 }
 344
 345 void trampQuery(uint8_t cmd)
 346 {
 347     trampResetReceiver();
 348     trampCmdU16(cmd, 0);
 349 }
 350
 351 void trampQueryR(void)
 352 {
 353     trampQuery('r');
 354 }
 355
 356 void trampQueryV(void)
 357 {
 358     trampQuery('v');
 359 }
 360
 361 void trampQueryS(void)
 362 {
 363     trampQuery('s');
 364 }
 365
 366 static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 367 {
 368     UNUSED(vtxDevice);
 369
 370     static timeUs_t lastQueryTimeUs = 0;
 371     static bool initSettingsDoneFlag = false;
 372
 373 #ifdef TRAMP_DEBUG
 374     static uint16_t debugFreqReqCounter = 0;
 375     static uint16_t debugPowReqCounter = 0;
 376 #endif
 377
 378     if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
 379         return;
 380     }
 381
 382     const char replyCode = trampReceive(currentTimeUs);
 383
 384 #ifdef TRAMP_DEBUG
 385     debug[0] = trampStatus;
 386 #endif
 387
 388     switch (replyCode) {
 389     case 'r':
 390         if (trampStatus <= TRAMP_STATUS_OFFLINE) {
 391             trampStatus = TRAMP_STATUS_ONLINE;
 392
 393             // once device is ready enter vtx settings
 394             if (!initSettingsDoneFlag) {
 395                 initSettingsDoneFlag = true;
 396                 // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
 397             }
 398         }
 399         break;
 400
 401     case 'v':
 402          if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
 403              trampStatus = TRAMP_STATUS_SET_FREQ_PW;
 404          }
 405          break;
 406     }
 407
 408     switch (trampStatus) {
 409
 410     case TRAMP_STATUS_OFFLINE:
 411     case TRAMP_STATUS_ONLINE:
 412         if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
 413
 414             if (trampStatus == TRAMP_STATUS_OFFLINE) {
 415                 trampQueryR();
 416             } else {
 417                 static unsigned int cnt = 0;
 418                 if (((cnt++) & 1) == 0) {
 419                     trampQueryV();
 420                 } else {
 421                     trampQueryS();
 422                 }
 423             }
 424
 425             lastQueryTimeUs = currentTimeUs;
 426         }
 427         break;
 428
 429     case TRAMP_STATUS_SET_FREQ_PW:
 430         {
 431             bool done = true;
 432             if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) {
 433                 trampSendFreq(trampConfFreq);
 434                 trampFreqRetries--;
 435 #ifdef TRAMP_DEBUG
 436                 debugFreqReqCounter++;
 437 #endif
 438                 done = false;
 439             } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
 440                 trampSendRFPower(trampConfPower);
 441                 trampPowerRetries--;
 442 #ifdef TRAMP_DEBUG
 443                 debugPowReqCounter++;
 444 #endif
 445                 done = false;
 446             }
 447
 448             if (!done) {
 449                 trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
 450
 451                 // delay next status query by 300ms
 452                 lastQueryTimeUs = currentTimeUs + 300 * 1000;
 453             } else {
 454                 // everything has been done, let's return to original state
 455                 trampStatus = TRAMP_STATUS_ONLINE;
 456                 // reset configuration value in case it failed (no more retries)
 457                 trampConfFreq  = trampCurFreq;
 458                 trampConfPower = trampPower;
 459                 trampFreqRetries = trampPowerRetries = 0;
 460             }
 461         }
 462         break;
 463
 464     case TRAMP_STATUS_CHECK_FREQ_PW:
 465         if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
 466             trampQueryV();
 467             lastQueryTimeUs = currentTimeUs;
 468         }
 469         break;
 470
 471     default:
 472         break;
 473     }
 474
 475 #ifdef TRAMP_DEBUG
 476     debug[1] = debugFreqReqCounter;
 477     debug[2] = debugPowReqCounter;
 478     debug[3] = 0;
 479 #endif
 480
 481 #ifdef USE_CMS
 482     trampCmsUpdateStatusString();
 483 #endif
 484 }
 485
 486
 487 #ifdef USE_VTX_COMMON
 488
 489 // Interface to common VTX API
 490
 491 static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
 492 {
 493     UNUSED(vtxDevice);
 494     return VTXDEV_TRAMP;
 495 }
 496
 497 static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
 498 {
 499     return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE;
 500 }
 501
 502 static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 503 {
 504     UNUSED(vtxDevice);
 505     if (trampValidateBandAndChannel(band, channel)) {
 506         trampSetBandAndChannel(band, channel);
 507         trampCommitChanges();
 508     }
 509 }
 510
 511 static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 512 {
 513     UNUSED(vtxDevice);
 514     trampDevSetPowerByIndex(index);
 515 }
 516
 517 static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 518 {
 519     UNUSED(vtxDevice);
 520     trampSetPitMode(onoff);
 521 }
 522
 523 static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 524 {
 525     UNUSED(vtxDevice);
 526     if (trampValidateFreq(freq)) {
 527         trampSetFreq(freq);
 528         trampCommitChanges();
 529     }
 530 }
 531
 532 static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 533 {
 534     if (!vtxTrampIsReady(vtxDevice)) {
 535         return false;
 536     }
 537
 538     // if in user-freq mode then report band as zero
 539     *pBand = trampSetByFreqFlag ? 0 : trampBand;
 540     *pChannel = trampChannel;
 541     return true;
 542 }
 543
 544 static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 545 {
 546     if (!vtxTrampIsReady(vtxDevice)) {
 547         return false;
 548     }
 549
 550     if (trampConfiguredPower > 0) {
 551         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
 552             if (trampConfiguredPower <= trampPowerTable[i]) {
 553                 *pIndex = i + 1;
 554                 break;
 555             }
 556         }
 557     }
 558
 559     return true;
 560 }
 561
 562 static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 563 {
 564     if (!vtxTrampIsReady(vtxDevice)) {
 565         return false;
 566     }
 567
 568     *pOnOff = trampPitMode;
 569     return true;
 570 }
 571
 572 static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 573 {
 574     if (!vtxTrampIsReady(vtxDevice)) {
 575         return false;
 576     }
 577
 578     *pFreq = trampCurFreq;
 579     return true;
 580 }
 581
 582 static const vtxVTable_t trampVTable = {
 583     .process = vtxTrampProcess,
 584     .getDeviceType = vtxTrampGetDeviceType,
 585     .isReady = vtxTrampIsReady,
 586     .setBandAndChannel = vtxTrampSetBandAndChannel,
 587     .setPowerByIndex = vtxTrampSetPowerByIndex,
 588     .setPitMode = vtxTrampSetPitMode,
 589     .setFrequency = vtxTrampSetFreq,
 590     .getBandAndChannel = vtxTrampGetBandAndChannel,
 591     .getPowerIndex = vtxTrampGetPowerIndex,
 592     .getPitMode = vtxTrampGetPitMode,
 593     .getFrequency = vtxTrampGetFreq,
 594 };
 595
 596 #endif
 597
 598 bool vtxTrampInit(void)
 599 {
 600     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
 601
 602     if (portConfig) {
 603         portOptions_e portOptions = 0;
 604 #if defined(USE_VTX_COMMON)
 605         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
 606 #else
 607         portOptions = SERIAL_BIDIR;
 608 #endif
 609
 610         trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
 611     }
 612
 613     if (!trampSerialPort) {
 614         return false;
 615     }
 616
 617 #if defined(USE_VTX_COMMON)
 618     vtxTramp.capability.bandCount = VTX_TRAMP_BAND_COUNT;
 619     vtxTramp.capability.channelCount = VTX_TRAMP_CHANNEL_COUNT;
 620     vtxTramp.capability.powerCount = sizeof(trampPowerTable),
 621     vtxTramp.frequencyTable = vtxStringFrequencyTable();
 622     vtxTramp.bandNames = vtxStringBandNames();
 623     vtxTramp.bandLetters = vtxStringBandLetters();
 624     vtxTramp.channelNames = vtxStringChannelNames();
 625     vtxTramp.powerNames = trampPowerNames;
 626     vtxTramp.powerValues = trampPowerTable;
 627
 628     vtxCommonSetDevice(&vtxTramp);
 629 #endif
 630
 631     return true;
 632 }
 633
 634 #endif // VTX_TRAMP