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