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