src/main/io/vtx_smartaudio.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 <string.h>
  26 #include <math.h>
  27 #include <ctype.h>
  28
  29 #include "platform.h"
  30
  31 #if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL)
  32
  33 #include "build/debug.h"
  34
  35 #include "cms/cms.h"
  36 #include "cms/cms_menu_vtx_smartaudio.h"
  37
  38 #include "common/maths.h"
  39 #include "common/printf.h"
  40 #include "common/utils.h"
  41
  42 #include "drivers/time.h"
  43 #include "drivers/vtx_common.h"
  44 #include "drivers/vtx_table.h"
  45
  46 #include "io/serial.h"
  47 #include "io/vtx.h"
  48 #include "io/vtx_control.h"
  49 #include "io/vtx_smartaudio.h"
  50
  51 // Timing parameters
  52 // Note that vtxSAProcess() is normally called at 200ms interval
  53 #define SMARTAUDIO_CMD_TIMEOUT       120    // Time until the command is considered lost
  54 #define SMARTAUDIO_POLLING_INTERVAL  150    // Minimum time between state polling
  55 #define SMARTAUDIO_POLLING_WINDOW   1000    // Time window after command polling for state change
  56
  57 #ifdef USE_SMARTAUDIO_DPRINTF
  58 serialPort_t *debugSerialPort = NULL;
  59 #endif // USE_SMARTAUDIO_DPRINTF
  60
  61 static serialPort_t *smartAudioSerialPort = NULL;
  62
  63 smartAudioDevice_t saDevice;
  64
  65 #ifdef USE_VTX_COMMON
  66 static const vtxVTable_t saVTable;    // Forward
  67 static vtxDevice_t vtxSmartAudio = {
  68     .vTable = &saVTable,
  69 };
  70 #endif
  71
  72 // SmartAudio command and response codes
  73 enum {
  74     SA_CMD_NONE = 0x00,
  75     SA_CMD_GET_SETTINGS = 0x01,
  76     SA_CMD_SET_POWER,
  77     SA_CMD_SET_CHAN,
  78     SA_CMD_SET_FREQ,
  79     SA_CMD_SET_MODE,
  80     SA_CMD_GET_SETTINGS_V2 = 0x09,        // Response only
  81     SA_CMD_GET_SETTINGS_V21 = 0x11,
  82 } smartAudioCommand_e;
  83
  84 // This is not a good design; can't distinguish command from response this way.
  85 #define SACMD(cmd) (((cmd) << 1) | 1)
  86
  87 #define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE)
  88 #define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE))
  89 #define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))
  90
  91 // convert between 'saDevice.channel' and band/channel values
  92 #define SA_DEVICE_CHVAL_TO_BAND(val) ((val) / (uint8_t)vtxTableChannelCount) + 1
  93 #define SA_DEVICE_CHVAL_TO_CHANNEL(val) ((val) % (uint8_t)vtxTableChannelCount) + 1
  94 #define SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel) ((band - 1) * (uint8_t)vtxTableChannelCount + (channel - 1))
  95
  96 // Statistical counters, for user side trouble shooting.
  97
  98 smartAudioStat_t saStat = {
  99     .pktsent = 0,
 100     .pktrcvd = 0,
 101     .badpre = 0,
 102     .badlen = 0,
 103     .crc = 0,
 104     .ooopresp = 0,
 105     .badcode = 0,
 106 };
 107
 108 // Last received device ('hard') states
 109
 110 smartAudioDevice_t saDevice = {
 111     .version = 0,
 112     .channel = -1,
 113     .power = 0,
 114     .mode = 0,
 115     .freq = 0,
 116     .orfreq = 0,
 117     .willBootIntoPitMode = false,
 118 };
 119
 120 static smartAudioDevice_t saDevicePrev = {
 121     .version = 0,
 122 };
 123
 124 // XXX Possible compliance problem here. Need LOCK/UNLOCK menu?
 125 static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
 126
 127 #ifdef USE_VTX_TABLE
 128 #define VTX_SMARTAUDIO_POWER_COUNT VTX_TABLE_MAX_POWER_LEVELS
 129 #else // USE_VTX_TABLE
 130 #define VTX_SMARTAUDIO_POWER_COUNT 4
 131 static char saSupportedPowerLabels[VTX_SMARTAUDIO_POWER_COUNT + 1][4] = {"---", "25 ", "200", "500", "800"};
 132 static char *saSupportedPowerLabelPointerArray[VTX_SMARTAUDIO_POWER_COUNT + 1];
 133 #endif // USE_VTX_TABLE
 134 static uint8_t saSupportedNumPowerLevels = VTX_SMARTAUDIO_POWER_COUNT;
 135 static uint16_t saSupportedPowerValues[VTX_SMARTAUDIO_POWER_COUNT];
 136
 137 // XXX Should be configurable by user?
 138 bool saDeferred = true; // saCms variable?
 139
 140 // Receive frame reassembly buffer
 141 #define SA_MAX_RCVLEN 21
 142 static uint8_t sa_rbuf[SA_MAX_RCVLEN + 4]; // XXX delete 4 byte guard
 143
 144 //
 145 // CRC8 computations
 146 //
 147
 148 #define POLYGEN 0xd5
 149
 150 static uint8_t CRC8(const uint8_t *data, const int8_t len)
 151 {
 152     uint8_t crc = 0; /* start with 0 so first byte can be 'xored' in */
 153     uint8_t currByte;
 154
 155     for (int i = 0 ; i < len ; i++) {
 156         currByte = data[i];
 157
 158         crc ^= currByte; /* XOR-in the next input byte */
 159
 160         for (int i = 0; i < 8; i++) {
 161             if ((crc & 0x80) != 0) {
 162                 crc = (uint8_t)((crc << 1) ^ POLYGEN);
 163             } else {
 164                 crc <<= 1;
 165             }
 166         }
 167     }
 168     return crc;
 169 }
 170
 171 #ifdef USE_SMARTAUDIO_DPRINTF
 172 static void saPrintSettings(void)
 173 {
 174     dprintf(("Current status: version: %d\r\n", saDevice.version));
 175     dprintf(("  mode(0x%x): fmode=%s", saDevice.mode,  (saDevice.mode & 1) ? "freq" : "chan"));
 176     dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
 177     dprintf((" inb=%s", (saDevice.mode & 4) ? "on " : "off"));
 178     dprintf((" outb=%s", (saDevice.mode & 8) ? "on " : "off"));
 179     dprintf((" lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked"));
 180     dprintf((" deferred=%s\r\n", (saDevice.mode & 32) ? "on" : "off"));
 181     dprintf(("  channel: %d ", saDevice.channel));
 182     dprintf(("freq: %d ", saDevice.freq));
 183     dprintf(("power: %d ", saDevice.power));
 184     dprintf(("powerval: %d ", saDevice.power > 0 ? vtxTablePowerValues[saDevice.power - 1] : -1));
 185     dprintf(("pitfreq: %d ", saDevice.orfreq));
 186     dprintf(("BootIntoPitMode: %s ", saDevice.willBootIntoPitMode ? "yes" : "no"));
 187     dprintf(("\r\n"));
 188 }
 189 #endif
 190
 191 //
 192 // Autobauding
 193 //
 194
 195 #define SMARTBAUD_MIN 4800
 196 #define SMARTBAUD_MAX 4950
 197 uint16_t sa_smartbaud = SMARTBAUD_MIN;
 198 static int sa_adjdir = 1; // -1=going down, 1=going up
 199 static int sa_baudstep = 50;
 200
 201 static void saAutobaud(void)
 202 {
 203     if (saStat.pktsent < 10) {
 204         // Not enough samples collected
 205         return;
 206     }
 207
 208 #if 0
 209     dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
 210              sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent)));
 211 #endif
 212
 213     if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) {
 214         // This is okay
 215         saStat.pktsent = 0; // Should be more moderate?
 216         saStat.pktrcvd = 0;
 217         return;
 218     }
 219
 220     dprintf(("autobaud: adjusting\r\n"));
 221
 222     if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) {
 223         sa_adjdir = -1;
 224         dprintf(("autobaud: now going down\r\n"));
 225     } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) {
 226         sa_adjdir = 1;
 227         dprintf(("autobaud: now going up\r\n"));
 228     }
 229
 230     sa_smartbaud += sa_baudstep * sa_adjdir;
 231
 232     dprintf(("autobaud: %d\r\n", sa_smartbaud));
 233
 234     smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud);
 235
 236     saStat.pktsent = 0;
 237     saStat.pktrcvd = 0;
 238 }
 239
 240 // Transport level variables
 241
 242 static timeUs_t sa_lastTransmissionMs = 0;
 243 static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
 244 static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
 245 static int sa_oslen;         // And associate length
 246
 247 static void saProcessResponse(uint8_t *buf, int len)
 248 {
 249     uint8_t resp = buf[0];
 250
 251     if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
 252         sa_outstanding = SA_CMD_NONE;
 253
 254         return;
 255     }
 256
 257     if (resp == sa_outstanding) {
 258         sa_outstanding = SA_CMD_NONE;
 259     } else if ((resp == SA_CMD_GET_SETTINGS_V2 ||
 260                 resp == SA_CMD_GET_SETTINGS_V21) &&
 261                (sa_outstanding == SA_CMD_GET_SETTINGS)) {
 262         sa_outstanding = SA_CMD_NONE;
 263     } else {
 264         saStat.ooopresp++;
 265         dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp));
 266     }
 267
 268     switch (resp) {
 269     case SA_CMD_GET_SETTINGS_V21: // Version 2.1 Get Settings
 270     case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
 271     case SA_CMD_GET_SETTINGS:    // Version 1 Get Settings
 272         dprintf(("received settings\r\n"));
 273         if (len < 7) {
 274             break;
 275         }
 276
 277         // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1"
 278         // saDevice.version = 0 means unknown, 1 means Smart audio V1, 2 means Smart audio V2 and 3 means Smart audio V2.1
 279         saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3);
 280         saDevice.channel = buf[2];
 281         uint8_t rawPowerValue = buf[3];
 282         saDevice.mode = buf[4];
 283         saDevice.freq = (buf[5] << 8) | buf[6];
 284
 285         // read pir and por flags to detect if the device will boot into pitmode.
 286         // note that "quit pitmode without unsetting the pitmode flag" clears pir and por flags but the device will still boot into pitmode.
 287         // therefore we ignore the pir and por flags while the device is not in pitmode
 288         // actually, this is the whole reason the variable saDevice.willBootIntoPitMode exists.
 289         // otherwise we could use saDevice.mode directly
 290         if (saDevice.mode & SA_MODE_GET_PITMODE) {
 291             bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE);
 292             if (newBootMode != saDevice.willBootIntoPitMode) {
 293                 dprintf(("saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode ? "true" : "false"));
 294             }
 295             saDevice.willBootIntoPitMode = newBootMode;
 296         }
 297
 298         if (saDevice.version == 3) {
 299             //read dbm based power levels
 300             //aaaaaand promptly forget them. todo: write them into vtxtable pg and load it
 301             if (len < 10) { //current power level in dbm field missing or power level length field missing or zero power levels reported
 302                 dprintf(("processResponse: V2.1 vtx didn't report any power levels\r\n"));
 303                 break;
 304             }
 305             saSupportedNumPowerLevels = constrain((int8_t)buf[8], 0, VTX_TABLE_MAX_POWER_LEVELS);
 306             //SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero.
 307             //zero is indeed a valid power level to set the vtx to, but it activates pit mode.
 308             //crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm.
 309             //instead, it reports whatever value was set previously and it reports to be in pit mode.
 310             //for this reason, zero shouldn't be used as a normal power level in betaflight.
 311 #ifdef USE_SMARTAUDIO_DPRINTF
 312             if ( len < ( 9 + saSupportedNumPowerLevels )) {
 313                 dprintf(("processResponse: V2.1 vtx expected %d power levels but packet too short\r\n", saSupportedNumPowerLevels));
 314                 break;
 315             }
 316             if (len > (10 + saSupportedNumPowerLevels )) {
 317                 dprintf(("processResponse: V2.1 %d extra bytes found!\r\n", len - (10 + saSupportedNumPowerLevels)));
 318             }
 319 #endif
 320             for ( int8_t i = 0; i < saSupportedNumPowerLevels; i++ ) {
 321                 saSupportedPowerValues[i] = buf[9 + i + 1];//+ 1 to skip the first power level, as mentioned above
 322             }
 323
 324             dprintf(("processResponse: %d power values: %d, %d, %d, %d\r\n",
 325                      vtxTablePowerLevels, vtxTablePowerValues[0], vtxTablePowerValues[1],
 326                      vtxTablePowerValues[2], vtxTablePowerValues[3]));
 327             //dprintf(("processResponse: V2.1 received vtx power value %d\r\n",buf[7]));
 328             rawPowerValue = buf[7];
 329         }
 330 #ifdef USE_SMARTAUDIO_DPRINTF
 331         int8_t prevPower = saDevice.power;
 332 #endif
 333         saDevice.power = 0;//set to unknown power level if the reported one doesnt match any of the known ones
 334         dprintf(("processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]));
 335         for (int8_t i = 0; i < vtxTablePowerLevels; i++) {
 336             if (rawPowerValue == vtxTablePowerValues[i]) {
 337 #ifdef USE_SMARTAUDIO_DPRINTF
 338                 if (prevPower != i + 1) {
 339                     dprintf(("processResponse: power changed from index %d to index %d\r\n", prevPower, i + 1));
 340                 }
 341 #endif
 342                 saDevice.power = i + 1;
 343
 344             }
 345         }
 346
 347         DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode);
 348         DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
 349         DEBUG_SET(DEBUG_SMARTAUDIO, 2, saDevice.freq);
 350         DEBUG_SET(DEBUG_SMARTAUDIO, 3, saDevice.power);
 351         break;
 352
 353     case SA_CMD_SET_POWER: // Set Power
 354         break;
 355
 356     case SA_CMD_SET_CHAN: // Set Channel
 357         break;
 358
 359     case SA_CMD_SET_FREQ: // Set Frequency
 360         if (len < 5) {
 361             break;
 362         }
 363
 364         const uint16_t freq = (buf[2] << 8) | buf[3];
 365
 366         if (freq & SA_FREQ_GETPIT) {
 367             saDevice.orfreq = freq & SA_FREQ_MASK;
 368             dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq));
 369         } else if (freq & SA_FREQ_SETPIT) {
 370             saDevice.orfreq = freq & SA_FREQ_MASK;
 371             dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq));
 372         } else {
 373             saDevice.freq = freq;
 374             dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq));
 375         }
 376         break;
 377
 378     case SA_CMD_SET_MODE: // Set Mode
 379         dprintf(("saProcessResponse: SET_MODE 0x%x (pir %s, por %s, pitdsbl %s, %s)\r\n",
 380                  buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off",
 381                  (buf[4] & 8) ? "unlocked" : "locked"));
 382         break;
 383
 384     default:
 385         saStat.badcode++;
 386         return;
 387     }
 388
 389     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
 390 #ifdef USE_CMS    //if changes then trigger saCms update
 391         saCmsResetOpmodel();
 392 #endif
 393 #ifdef USE_SMARTAUDIO_DPRINTF    // Debug
 394         saPrintSettings();
 395 #endif
 396     }
 397     saDevicePrev = saDevice;
 398
 399 #ifdef USE_VTX_COMMON
 400     // Todo: Update states in saVtxDevice?
 401 #endif
 402
 403 #ifdef USE_CMS
 404     // Export current device status for CMS
 405     saCmsUpdate();
 406     saUpdateStatusString();
 407 #endif
 408 }
 409
 410 //
 411 // Datalink
 412 //
 413
 414 static void saReceiveFrame(uint8_t c)
 415 {
 416
 417     static enum saFramerState_e {
 418         S_WAITPRE1, // Waiting for preamble 1 (0xAA)
 419         S_WAITPRE2, // Waiting for preamble 2 (0x55)
 420         S_WAITRESP, // Waiting for response code
 421         S_WAITLEN,  // Waiting for length
 422         S_DATA,     // Receiving data
 423         S_WAITCRC,  // Waiting for CRC
 424     } state = S_WAITPRE1;
 425
 426     static int len;
 427     static int dlen;
 428
 429     switch (state) {
 430     case S_WAITPRE1:
 431         if (c == 0xAA) {
 432             state = S_WAITPRE2;
 433         } else {
 434             state = S_WAITPRE1; // Don't need this (no change)
 435         }
 436         break;
 437
 438     case S_WAITPRE2:
 439         if (c == 0x55) {
 440             state = S_WAITRESP;
 441         } else {
 442             saStat.badpre++;
 443             state = S_WAITPRE1;
 444         }
 445         break;
 446
 447     case S_WAITRESP:
 448         sa_rbuf[0] = c;
 449         state = S_WAITLEN;
 450         break;
 451
 452     case S_WAITLEN:
 453         sa_rbuf[1] = c;
 454         len = c;
 455
 456         if (len > SA_MAX_RCVLEN - 2) {
 457             saStat.badlen++;
 458             state = S_WAITPRE1;
 459         } else if (len == 0) {
 460             state = S_WAITCRC;
 461         } else {
 462             dlen = 0;
 463             state = S_DATA;
 464         }
 465         break;
 466
 467     case S_DATA:
 468         // XXX Should check buffer overflow (-> saerr_overflow)
 469         sa_rbuf[2 + dlen] = c;
 470         if (++dlen == len) {
 471             state = S_WAITCRC;
 472         }
 473         break;
 474
 475     case S_WAITCRC:
 476         if (CRC8(sa_rbuf, 2 + len) == c) {
 477             // Got a response
 478             saProcessResponse(sa_rbuf, len + 2);
 479             saStat.pktrcvd++;
 480         } else if (sa_rbuf[0] & 1) {
 481             // Command echo
 482             // XXX There is an exceptional case (V2 response)
 483             // XXX Should check crc in the command format?
 484         } else {
 485             saStat.crc++;
 486         }
 487         state = S_WAITPRE1;
 488         break;
 489     }
 490 }
 491
 492 static void saSendFrame(uint8_t *buf, int len)
 493 {
 494     if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
 495         bool prepend00;
 496         switch (serialType(smartAudioSerialPort->identifier)) {
 497         case SERIALTYPE_SOFTSERIAL:
 498             prepend00 = vtxSettingsConfig()->softserialAlt;
 499             break;
 500         case SERIALTYPE_UART:
 501         case SERIALTYPE_LPUART: // decide HW uarts by MCU type
 502 #ifdef AT32F4
 503             prepend00 = false;
 504 #else
 505             prepend00 = true;
 506 #endif
 507             break;
 508         default:
 509             prepend00 = false;
 510         }
 511         if (prepend00) {
 512             // line is in MARK/BREAK, so only 2 stopbits will be visible (startbit and zeroes are not visible)
 513             // startbit of next byte (0xaa) can be recognized
 514             serialWrite(smartAudioSerialPort, 0x00);
 515         }
 516
 517         for (int i = 0 ; i < len ; i++) {
 518             serialWrite(smartAudioSerialPort, buf[i]);
 519         }
 520 #ifdef USE_AKK_SMARTAUDIO
 521         serialWrite(smartAudioSerialPort, 0x00); // AKK/RDQ SmartAudio devices can expect an extra byte due to manufacturing errors.
 522 #endif
 523
 524         saStat.pktsent++;
 525     } else {
 526         sa_outstanding = SA_CMD_NONE;
 527     }
 528
 529     sa_lastTransmissionMs = millis();
 530 }
 531
 532 /*
 533  * Retransmission and command queuing
 534  *
 535  *   The transport level support includes retransmission on response timeout
 536  * and command queueing.
 537  *
 538  * Resend buffer:
 539  *   The smartaudio returns response for valid command frames in no less
 540  * than 60msec, which we can't wait. So there's a need for a resend buffer.
 541  *
 542  * Command queueing:
 543  *   The driver autonomously sends GetSettings command for auto-bauding,
 544  * asynchronous to user initiated commands; commands issued while another
 545  * command is outstanding must be queued for later processing.
 546  *   The queueing also handles the case in which multiple commands are
 547  * required to implement a user level command.
 548  */
 549
 550 // Retransmission
 551
 552 static void saResendCmd(void)
 553 {
 554     saSendFrame(sa_osbuf, sa_oslen);
 555 }
 556
 557 static void saSendCmd(const uint8_t *buf, int len)
 558 {
 559     for (int i = 0 ; i < len ; i++) {
 560         sa_osbuf[i] = buf[i];
 561     }
 562
 563     sa_oslen = len;
 564     sa_outstanding = (buf[2] >> 1);
 565
 566     saSendFrame(sa_osbuf, sa_oslen);
 567 }
 568
 569 // Command queue management
 570
 571 typedef struct saCmdQueue_s {
 572     uint8_t *buf;
 573     int len;
 574 } saCmdQueue_t;
 575
 576 #define SA_QSIZE 6     // 1 heartbeat (GetSettings) + 2 commands + 1 slack
 577 static saCmdQueue_t sa_queue[SA_QSIZE];
 578 static uint8_t sa_qhead = 0;
 579 static uint8_t sa_qtail = 0;
 580
 581 static bool saQueueEmpty(void)
 582 {
 583     return sa_qhead == sa_qtail;
 584 }
 585
 586 static bool saQueueFull(void)
 587 {
 588     return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail;
 589 }
 590
 591 static void saQueueCmd(uint8_t *buf, int len)
 592 {
 593     if (saQueueFull()) {
 594         return;
 595     }
 596
 597     sa_queue[sa_qhead].buf = buf;
 598     sa_queue[sa_qhead].len = len;
 599     sa_qhead = (sa_qhead + 1) % SA_QSIZE;
 600 }
 601
 602 static void saSendQueue(void)
 603 {
 604     if (saQueueEmpty()) {
 605         return;
 606     }
 607
 608     saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
 609     sa_qtail = (sa_qtail + 1) % SA_QSIZE;
 610 }
 611
 612 // Individual commands
 613
 614 static void saGetSettings(void)
 615 {
 616     static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
 617
 618     dprintf(("smartAudioGetSettings\r\n"));
 619     saQueueCmd(bufGetSettings, 5);
 620 }
 621
 622 static bool saValidateFreq(uint16_t freq)
 623 {
 624     return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ);
 625 }
 626
 627 void saSetFreq(uint16_t freq)
 628 {
 629     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
 630     static uint8_t switchBuf[7];
 631
 632     if (freq & SA_FREQ_GETPIT) {
 633         dprintf(("smartAudioSetFreq: GETPIT\r\n"));
 634     } else if (freq & SA_FREQ_SETPIT) {
 635         dprintf(("smartAudioSetFreq: SETPIT %d\r\n", freq & SA_FREQ_MASK));
 636     } else {
 637         dprintf(("smartAudioSetFreq: SET %d\r\n", freq));
 638     }
 639
 640     buf[4] = (freq >> 8) & 0xff;
 641     buf[5] = freq & 0xff;
 642     buf[6] = CRC8(buf, 6);
 643
 644     // Need to work around apparent SmartAudio bug when going from 'channel'
 645     // to 'user-freq' mode, where the set-freq command will fail if the freq
 646     // value is unchanged from the previous 'user-freq' mode
 647     if ((saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) == 0 && freq == saDevice.freq) {
 648         memcpy(&switchBuf, &buf, sizeof(buf));
 649         const uint16_t switchFreq = freq + ((freq == VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ) ? -1 : 1);
 650         switchBuf[4] = (switchFreq >> 8);
 651         switchBuf[5] = switchFreq & 0xff;
 652         switchBuf[6] = CRC8(switchBuf, 6);
 653
 654         saQueueCmd(switchBuf, 7);
 655
 656         // need to do a 'get' between the 'set' commands to keep tracking vars in sync
 657         saGetSettings();
 658     }
 659
 660     saQueueCmd(buf, 7);
 661 }
 662
 663 void saSetPitFreq(uint16_t freq)
 664 {
 665     saSetFreq(freq | SA_FREQ_SETPIT);
 666 }
 667
 668 #if 0
 669 static void saGetPitFreq(void)
 670 {
 671     saDoDevSetFreq(SA_FREQ_GETPIT);
 672 }
 673 #endif
 674
 675 void saSetMode(int mode)
 676 {
 677     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
 678
 679     buf[4] = (mode & 0x3f) | saLockMode;
 680     if (saDevice.version >= 3 && (mode & SA_MODE_CLR_PITMODE) &&
 681         ((mode & SA_MODE_SET_IN_RANGE_PITMODE) || (mode & SA_MODE_SET_OUT_RANGE_PITMODE))) {
 682         saDevice.willBootIntoPitMode = true;//quit pitmode without unsetting flag.
 683         //the response will just say pit=off but the device will still go into pitmode on reboot.
 684         //therefore we have to memorize this change here.
 685     }
 686     dprintf(("saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
 687             (mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked"));
 688     buf[5] = CRC8(buf, 5);
 689
 690     saQueueCmd(buf, 6);
 691 }
 692
 693 bool vtxSmartAudioInit(void)
 694 {
 695 #if !defined(USE_VTX_TABLE)
 696     for (int8_t i = 0; i < VTX_SMARTAUDIO_POWER_COUNT + 1; i++) {
 697         saSupportedPowerLabelPointerArray[i] = saSupportedPowerLabels[i];
 698     }
 699 #endif
 700 #ifdef USE_SMARTAUDIO_DPRINTF
 701     // Setup debugSerialPort
 702
 703     debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, NULL, 115200, MODE_RXTX, 0);
 704     if (debugSerialPort) {
 705         setPrintfSerialPort(debugSerialPort);
 706     }
 707     dprintf(("smartAudioInit: OK\r\n"));
 708 #endif
 709
 710     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
 711     if (!portConfig) {
 712         return false;
 713     }
 714     // Note, for SA, which uses bidirectional mode, would normally require pullups.
 715     // the SA protocol usually requires pulldowns, and therefore uses SERIAL_PULL_SMARTAUDIO together with SERIAL_BIDIR_PP
 716     // serial driver handles different pullup/pulldown/nopull quirks when SERIAL_PULL_SMARTAUDIO is used
 717     const portOptions_e portOptions = SERIAL_NOT_INVERTED | SERIAL_STOPBITS_2 | SERIAL_BIDIR | SERIAL_BIDIR_PP | SERIAL_PULL_SMARTAUDIO;
 718     smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions);
 719
 720     if (!smartAudioSerialPort) {
 721         return false;
 722     }
 723
 724     for (int8_t i = 0; i < VTX_SMARTAUDIO_POWER_COUNT; i++) {
 725         saSupportedPowerValues[i] = 1;
 726     }
 727
 728     dprintf(("vtxSmartAudioInit %d power levels recorded\r\n", vtxTablePowerLevels));
 729
 730     vtxCommonSetDevice(&vtxSmartAudio);
 731 #ifndef USE_VTX_TABLE
 732     vtxTableSetFactoryBands(true);
 733 #endif
 734
 735     vtxInit();
 736
 737     return true;
 738 }
 739
 740 #define SA_INITPHASE_START         0
 741 #define SA_INITPHASE_WAIT_SETTINGS 1 // SA_CMD_GET_SETTINGS was sent and waiting for reply.
 742 #define SA_INITPHASE_WAIT_PITFREQ  2 // SA_FREQ_GETPIT sent and waiting for reply.
 743 #define SA_INITPHASE_DONE          3
 744
 745 static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 746 {
 747     UNUSED(vtxDevice);
 748     UNUSED(currentTimeUs);
 749
 750     static char initPhase = SA_INITPHASE_START;
 751
 752     if (smartAudioSerialPort == NULL) {
 753         return;
 754     }
 755
 756     while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
 757         uint8_t c = serialRead(smartAudioSerialPort);
 758         saReceiveFrame((uint16_t)c);
 759     }
 760
 761     // Re-evaluate baudrate after each frame reception
 762     saAutobaud();
 763
 764     switch (initPhase) {
 765     case SA_INITPHASE_START:
 766         saGetSettings();
 767         //saSendQueue();
 768         initPhase = SA_INITPHASE_WAIT_SETTINGS;
 769         break;
 770
 771     case SA_INITPHASE_WAIT_SETTINGS:
 772         // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
 773         // and put the device into user frequency mode with uninitialized freq.
 774         // Also don't send it to V2.1 for the same reason.
 775         if (saDevice.version) {
 776             if (saDevice.version == 2) {
 777                 saSetFreq(SA_FREQ_GETPIT);
 778                 initPhase = SA_INITPHASE_WAIT_PITFREQ;
 779             } else {
 780                 initPhase = SA_INITPHASE_DONE;
 781             }
 782
 783 #if !defined(USE_VTX_TABLE)
 784             if (saDevice.version == 1) {//this is kind of ugly. use fixed tables and set a pointer to them instead?
 785                 saSupportedPowerValues[0] = 7;
 786                 saSupportedPowerValues[1] = 16;
 787                 saSupportedPowerValues[2] = 25;
 788                 saSupportedPowerValues[3] = 40;
 789             } else if (saDevice.version == 2) {
 790                 saSupportedPowerValues[0] = 0;
 791                 saSupportedPowerValues[1] = 1;
 792                 saSupportedPowerValues[2] = 2;
 793                 saSupportedPowerValues[3] = 3;
 794             }
 795
 796             //without USE_VTX_TABLE, fill vtxTable variables with default settings (instead of loading them from PG)
 797             vtxTablePowerLevels = constrain(saSupportedNumPowerLevels, 0, VTX_SMARTAUDIO_POWER_COUNT);
 798             if (saDevice.version >= 3) {
 799                 for (int8_t i = 0; i < vtxTablePowerLevels; i++) {
 800                     //ideally we would convert dbm to mW here
 801                     tfp_sprintf(saSupportedPowerLabels[i + 1], "%3d", constrain(saSupportedPowerValues[i], 0, 999));
 802                 }
 803             }
 804             for (int8_t i = 0; i < vtxTablePowerLevels; i++) {
 805                 vtxTablePowerValues[i] = saSupportedPowerValues[i];
 806             }
 807             for (int8_t i = 0; i < vtxTablePowerLevels + 1; i++) {
 808                 vtxTablePowerLabels[i] = saSupportedPowerLabels[i];
 809             }
 810             dprintf(("vtxSAProcess init phase vtxTablePowerLevels set to %d\r\n", vtxTablePowerLevels));
 811 #endif
 812
 813             if (saDevice.version >= 2 ) {
 814                 //did the device boot up in pit mode on its own?
 815                 saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false;
 816                 dprintf(("sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false"));
 817             }
 818         }
 819         break;
 820
 821     case SA_INITPHASE_WAIT_PITFREQ:
 822         if (saDevice.orfreq) {
 823             initPhase = SA_INITPHASE_DONE;
 824         }
 825         break;
 826
 827     case SA_INITPHASE_DONE:
 828         break;
 829     }
 830
 831     // Command queue control
 832
 833     timeMs_t nowMs = millis();             // Don't substitute with "currentTimeUs / 1000"; sa_lastTransmissionMs is based on millis().
 834     static timeMs_t lastCommandSentMs = 0; // Last non-GET_SETTINGS sent
 835
 836     if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) {
 837         // Last command timed out
 838         dprintf(("process: resending 0x%x\r\n", sa_outstanding));
 839         // XXX Todo: Resend termination and possible offline transition
 840         saResendCmd();
 841         lastCommandSentMs = nowMs;
 842     } else if (!saQueueEmpty()) {
 843         // Command pending. Send it.
 844         dprintf(("process: sending queue\r\n"));
 845         saSendQueue();
 846         lastCommandSentMs = nowMs;
 847     } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW)
 848                && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
 849         dprintf(("process: sending status change polling\r\n"));
 850         saGetSettings();
 851         saSendQueue();
 852     }
 853 }
 854 #ifdef USE_VTX_COMMON
 855 // Interface to common VTX API
 856
 857 vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
 858 {
 859     UNUSED(vtxDevice);
 860     return VTXDEV_SMARTAUDIO;
 861 }
 862
 863 static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
 864 {
 865 #ifndef USE_VTX_TABLE
 866     if (vtxDevice != NULL && saDevice.power == 0) {
 867         return false;
 868         //wait until power reading exists
 869         //this is needed bc the powervalues are loaded into vtxTableXXX after the first settings are received
 870         //thus the first time processResponse runs its index lookup it won't find anything and no power is recorded
 871         //this waits until after the second time settings are received which will actually find something
 872         //this check is not needed with USE_VTX_TABLE as the table is loaded from pg before smartaudio is even started
 873         //in fact, with USE_VTX_TABLE this check could end up paralyzing the smartaudio implementation if the user has
 874         //chosen to omit a power state in the vtxtable but the vtx happens to power up in that state for whatever reason
 875     }
 876 #endif
 877     return vtxDevice != NULL && saDevice.version != 0;
 878 }
 879
 880 static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
 881 {
 882     return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= vtxTableBandCount &&
 883             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= vtxTableChannelCount);
 884 }
 885
 886 static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 887 {
 888     UNUSED(vtxDevice);
 889     if (saValidateBandAndChannel(band, channel)) {
 890         static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 891
 892         buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
 893         buf[5] = CRC8(buf, 5);
 894         dprintf(("vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]));
 895
 896         //this will clear saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ
 897         saQueueCmd(buf, 6);
 898     }
 899 }
 900
 901 static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 902 {
 903     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
 904
 905     if (!vtxSAIsReady(vtxDevice)) {
 906         return;
 907     }
 908
 909     uint16_t powerValue = 0;
 910     if (!vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
 911         dprintf(("saSetPowerByIndex: cannot get power level %d, only levels 1 through %d supported", index,
 912                  vtxTablePowerLevels));
 913         return;
 914     }
 915
 916     buf[4] = powerValue;
 917     dprintf(("saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]));
 918     if (saDevice.version == 3) {
 919         buf[4] |= 128;//set MSB to indicate set power by dbm
 920     }
 921     buf[5] = CRC8(buf, 5);
 922     saQueueCmd(buf, 6);
 923 }
 924
 925 static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 926 {
 927     if (!vtxSAIsReady(vtxDevice) || saDevice.version < 2) {
 928         return;
 929     }
 930
 931     if (onoff && saDevice.version < 3) {
 932         // Smart Audio prior to V2.1 can not turn pit mode on by software.
 933         return;
 934     }
 935
 936     if (saDevice.version >= 3 && !saDevice.willBootIntoPitMode) {
 937         if (onoff) {
 938             // enable pitmode using SET_POWER command with 0 dbm.
 939             // This enables pitmode without causing the device to boot into pitmode next power-up
 940             static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
 941             buf[4] = 0 | 128;
 942             buf[5] = CRC8(buf, 5);
 943             saQueueCmd(buf, 6);
 944             dprintf(("vtxSASetPitMode: set power to 0 dbm\r\n"));
 945         } else {
 946             saSetMode(SA_MODE_CLR_PITMODE);
 947             dprintf(("vtxSASetPitMode: clear pitmode permanently"));
 948         }
 949         return;
 950     }
 951
 952     uint8_t newMode = onoff ? 0 : SA_MODE_CLR_PITMODE;
 953
 954     if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
 955         newMode |= SA_MODE_SET_OUT_RANGE_PITMODE;
 956     }
 957
 958     if ((saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (onoff && newMode == 0)) {
 959         // ensure when turning on pit mode that pit mode gets actually enabled
 960         newMode |= SA_MODE_SET_IN_RANGE_PITMODE;
 961     }
 962     dprintf(("vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
 963             (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off",
 964             (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode));
 965
 966     saSetMode(newMode);
 967
 968     return;
 969 }
 970
 971 static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 972 {
 973     if (!vtxSAIsReady(vtxDevice)) {
 974         return;
 975     }
 976
 977     if (saValidateFreq(freq)) {
 978         saSetFreq(freq);
 979     }
 980 }
 981
 982 static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 983 {
 984     if (!vtxSAIsReady(vtxDevice)) {
 985         return false;
 986     }
 987
 988     if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) {
 989         *pBand = 0;
 990         *pChannel = 0;
 991         return true;
 992     } else {
 993         *pBand = SA_DEVICE_CHVAL_TO_BAND(saDevice.channel);
 994         *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel);
 995         return true;
 996     }
 997 }
 998
 999 static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
1000 {
1001     if (!vtxSAIsReady(vtxDevice)) {
1002         return false;
1003     }
1004
1005     *pIndex = saDevice.power;//power levels are 1-based to match vtxtables with one more label than value
1006     return true;
1007 }
1008
1009 static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
1010 {
1011     if (!vtxSAIsReady(vtxDevice)) {
1012         return false;
1013     }
1014
1015     // if not in user-freq mode then convert band/chan to frequency
1016     *pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq :
1017              vtxCommonLookupFrequency(&vtxSmartAudio,
1018                                       SA_DEVICE_CHVAL_TO_BAND(saDevice.channel),
1019                                       SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel));
1020     return true;
1021 }
1022
1023 static bool vtxSAGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
1024 {
1025     if (!vtxSAIsReady(vtxDevice) || saDevice.version < 2) {
1026         return false;
1027     }
1028
1029     *status = (saDevice.mode & SA_MODE_GET_PITMODE) ? VTX_STATUS_PIT_MODE : 0;
1030
1031     return true;
1032 }
1033
1034 static uint8_t vtxSAGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
1035 {
1036     if (!vtxSAIsReady(vtxDevice) || saDevice.version < 2) {
1037         return 0;
1038     }
1039
1040     for (uint8_t i = 0; i < saSupportedNumPowerLevels; i++) {
1041         levels[i] = saSupportedPowerValues[i];
1042         uint16_t power = (uint16_t)powf(10.0f, levels[i] / 10.0f);
1043
1044         if (levels[i] > 14) {
1045             // For powers greater than 25mW round up to a multiple of 50 to match expectations
1046             power = 50 * ((power + 25) / 50);
1047         }
1048
1049         powers[i] = power;
1050     }
1051
1052     return saSupportedNumPowerLevels;
1053 }
1054
1055 #define VTX_CUSTOM_DEVICE_STATUS_SIZE 5
1056
1057 static void vtxSASerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
1058 {
1059     UNUSED(vtxDevice);
1060     sbufWriteU8(dst, VTX_CUSTOM_DEVICE_STATUS_SIZE);
1061     sbufWriteU8(dst, saDevice.version);
1062     sbufWriteU8(dst, saDevice.mode);
1063     sbufWriteU16(dst, saDevice.orfreq); // pit frequency
1064     sbufWriteU8(dst, saDevice.willBootIntoPitMode);
1065 }
1066
1067 #undef VTX_CUSTOM_DEVICE_STATUS_SIZE
1068
1069 static const vtxVTable_t saVTable = {
1070     .process = vtxSAProcess,
1071     .getDeviceType = vtxSAGetDeviceType,
1072     .isReady = vtxSAIsReady,
1073     .setBandAndChannel = vtxSASetBandAndChannel,
1074     .setPowerByIndex = vtxSASetPowerByIndex,
1075     .setPitMode = vtxSASetPitMode,
1076     .setFrequency = vtxSASetFreq,
1077     .getBandAndChannel = vtxSAGetBandAndChannel,
1078     .getPowerIndex = vtxSAGetPowerIndex,
1079     .getFrequency = vtxSAGetFreq,
1080     .getStatus = vtxSAGetStatus,
1081     .getPowerLevels = vtxSAGetPowerLevels,
1082     .serializeCustomDeviceStatus = vtxSASerializeCustomDeviceStatus,
1083 };
1084 #endif // VTX_COMMON
1085
1086 #endif // VTX_SMARTAUDIO