src/main/io/vtx_smartaudio.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /* Created by jflyper */
  19
  20 #include <stdbool.h>
  21 #include <stdint.h>
  22 #include <string.h>
  23 #include <ctype.h>
  24
  25 #include "platform.h"
  26
  27 #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
  28 #include "build/build_config.h"
  29
  30 #include "cms/cms.h"
  31 #include "cms/cms_types.h"
  32
  33 #include "common/printf.h"
  34 #include "common/utils.h"
  35
  36 #include "config/parameter_group.h"
  37 #include "config/parameter_group_ids.h"
  38
  39 #include "drivers/system.h"
  40 #include "drivers/serial.h"
  41 #include "drivers/vtx_common.h"
  42
  43 #include "fc/rc_controls.h"
  44 #include "fc/runtime_config.h"
  45
  46 #include "flight/pid.h"
  47
  48 #include "io/serial.h"
  49 #include "io/vtx_smartaudio.h"
  50 #include "io/vtx_string.h"
  51
  52 //#define SMARTAUDIO_DPRINTF
  53 //#define SMARTAUDIO_DEBUG_MONITOR
  54
  55 #ifdef SMARTAUDIO_DPRINTF
  56
  57 #ifdef OMNIBUSF4
  58 #define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
  59 #else
  60 #define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1
  61 #endif
  62
  63 serialPort_t *debugSerialPort = NULL;
  64 #define dprintf(x) if (debugSerialPort) printf x
  65 #else
  66 #define dprintf(x)
  67 #endif
  68
  69 #include "build/debug.h"
  70
  71 #ifdef CMS
  72 static void saUpdateStatusString(void); // Forward
  73 #endif
  74
  75 static serialPort_t *smartAudioSerialPort = NULL;
  76
  77 #if defined(CMS) || defined(VTX_COMMON)
  78 static const char * const saPowerNames[] = {
  79     "---", "25 ", "200", "500", "800",
  80 };
  81 #endif
  82
  83 #ifdef VTX_COMMON
  84 static const vtxVTable_t saVTable;    // Forward
  85 static vtxDevice_t vtxSmartAudio = {
  86     .vTable = &saVTable,
  87     .capability.bandCount = 5,
  88     .capability.channelCount = 8,
  89     .capability.powerCount = 4,
  90     .bandNames = (char **)vtx58BandNames,
  91     .channelNames = (char **)vtx58ChannelNames,
  92     .powerNames = (char **)saPowerNames,
  93 };
  94 #endif
  95
  96 // SmartAudio command and response codes
  97 enum {
  98     SA_CMD_NONE = 0x00,
  99     SA_CMD_GET_SETTINGS = 0x01,
 100     SA_CMD_SET_POWER,
 101     SA_CMD_SET_CHAN,
 102     SA_CMD_SET_FREQ,
 103     SA_CMD_SET_MODE,
 104     SA_CMD_GET_SETTINGS_V2 = 0x09        // Response only
 105 } smartAudioCommand_e;
 106
 107 // This is not a good design; can't distinguish command from response this way.
 108 #define SACMD(cmd) (((cmd) << 1) | 1)
 109
 110 // opmode flags, GET side
 111 #define SA_MODE_GET_FREQ_BY_FREQ            1
 112 #define SA_MODE_GET_PITMODE                 2
 113 #define SA_MODE_GET_IN_RANGE_PITMODE        4
 114 #define SA_MODE_GET_OUT_RANGE_PITMODE       8
 115 #define SA_MODE_GET_UNLOCK                 16
 116 #define SA_MODE_GET_DEFERRED_FREQ          32
 117
 118 #define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE)
 119 #define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE))
 120 #define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))
 121
 122 // opmode flags, SET side
 123 #define SA_MODE_SET_IN_RANGE_PITMODE        1 // Immediate
 124 #define SA_MODE_SET_OUT_RANGE_PITMODE        2 // Immediate
 125 #define SA_MODE_CLR_PITMODE                 4 // Immediate
 126 #define SA_MODE_SET_UNLOCK                  8
 127 #define SA_MODE_SET_LOCK                    0 // ~UNLOCK
 128 #define SA_MODE_SET_DEFERRED_FREQ          16
 129
 130 // SetFrequency flags, for pit mode frequency manipulation
 131 #define SA_FREQ_GETPIT                      (1 << 14)
 132 #define SA_FREQ_SETPIT                      (1 << 15)
 133 #define SA_FREQ_MASK                        (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
 134
 135 // Statistical counters, for user side trouble shooting.
 136
 137 typedef struct smartAudioStat_s {
 138     uint16_t pktsent;
 139     uint16_t pktrcvd;
 140     uint16_t badpre;
 141     uint16_t badlen;
 142     uint16_t crc;
 143     uint16_t ooopresp;
 144     uint16_t badcode;
 145 } smartAudioStat_t;
 146
 147 static smartAudioStat_t saStat = {
 148     .pktsent = 0,
 149     .pktrcvd = 0,
 150     .badpre = 0,
 151     .badlen = 0,
 152     .crc = 0,
 153     .ooopresp = 0,
 154     .badcode = 0,
 155 };
 156
 157 typedef struct saPowerTable_s {
 158     int rfpower;
 159     int16_t valueV1;
 160     int16_t valueV2;
 161 } saPowerTable_t;
 162
 163 static saPowerTable_t saPowerTable[] = {
 164     {  25,   7,   0 },
 165     { 200,  16,   1 },
 166     { 500,  25,   2 },
 167     { 800,  40,   3 },
 168 };
 169
 170 // Last received device ('hard') states
 171
 172 typedef struct smartAudioDevice_s {
 173     int8_t version;
 174     int8_t channel;
 175     int8_t power;
 176     int8_t mode;
 177     uint16_t freq;
 178     uint16_t orfreq;
 179 } smartAudioDevice_t;
 180
 181 static smartAudioDevice_t saDevice = {
 182     .version = 0,
 183     .channel = -1,
 184     .power = -1,
 185     .mode = 0,
 186     .freq = 0,
 187     .orfreq = 0,
 188 };
 189
 190 static smartAudioDevice_t saDevicePrev = {
 191     .version = 0,
 192 };
 193
 194 // XXX Possible compliance problem here. Need LOCK/UNLOCK menu?
 195 static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
 196
 197 // XXX Should be configurable by user?
 198 static bool saDeferred = true; // saCms variable?
 199
 200 // Receive frame reassembly buffer
 201 #define SA_MAX_RCVLEN 11
 202 static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
 203
 204 //
 205 // CRC8 computations
 206 //
 207
 208 #define POLYGEN 0xd5
 209
 210 static uint8_t CRC8(const uint8_t *data, const int8_t len)
 211 {
 212     uint8_t crc = 0; /* start with 0 so first byte can be 'xored' in */
 213     uint8_t currByte;
 214
 215     for (int i = 0 ; i < len ; i++) {
 216         currByte = data[i];
 217
 218         crc ^= currByte; /* XOR-in the next input byte */
 219
 220         for (int i = 0; i < 8; i++) {
 221             if ((crc & 0x80) != 0) {
 222                 crc = (uint8_t)((crc << 1) ^ POLYGEN);
 223             } else {
 224                 crc <<= 1;
 225             }
 226         }
 227     }
 228     return crc;
 229 }
 230
 231
 232 static void saPrintSettings(void)
 233 {
 234 #ifdef SMARTAUDIO_DPRINTF
 235     dprintf(("Current status: version: %d\r\n", saDevice.version));
 236     dprintf(("  mode(0x%x): fmode=%s", saDevice.mode,  (saDevice.mode & 1) ? "freq" : "chan"));
 237     dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
 238     dprintf((" inb=%s", (saDevice.mode & 4) ? "on " : "off"));
 239     dprintf((" outb=%s", (saDevice.mode & 8) ? "on " : "off"));
 240     dprintf((" lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked"));
 241     dprintf((" deferred=%s\r\n", (saDevice.mode & 32) ? "on" : "off"));
 242     dprintf(("  channel: %d ", saDevice.channel));
 243     dprintf(("freq: %d ", saDevice.freq));
 244     dprintf(("power: %d ", saDevice.power));
 245     dprintf(("pitfreq: %d ", saDevice.orfreq));
 246     dprintf(("\r\n"));
 247 #endif
 248 }
 249
 250 static int saDacToPowerIndex(int dac)
 251 {
 252     int idx;
 253
 254     for (idx = 3 ; idx >= 0 ; idx--) {
 255         if (saPowerTable[idx].valueV1 <= dac)
 256             return(idx);
 257     }
 258     return(0);
 259 }
 260
 261 //
 262 // Autobauding
 263 //
 264
 265 #define SMARTBAUD_MIN 4800
 266 #define SMARTBAUD_MAX 4950
 267 uint16_t sa_smartbaud = SMARTBAUD_MIN;
 268 static int sa_adjdir = 1; // -1=going down, 1=going up
 269 static int sa_baudstep = 50;
 270
 271 #define SMARTAUDIO_CMD_TIMEOUT    120
 272
 273 static void saAutobaud(void)
 274 {
 275     if (saStat.pktsent < 10)
 276         // Not enough samples collected
 277         return;
 278
 279 #if 0
 280     dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
 281         sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent)));
 282 #endif
 283
 284     if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) {
 285         // This is okay
 286         saStat.pktsent = 0; // Should be more moderate?
 287         saStat.pktrcvd = 0;
 288         return;
 289     }
 290
 291     dprintf(("autobaud: adjusting\r\n"));
 292
 293     if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) {
 294        sa_adjdir = -1;
 295        dprintf(("autobaud: now going down\r\n"));
 296     } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) {
 297        sa_adjdir = 1;
 298        dprintf(("autobaud: now going up\r\n"));
 299     }
 300
 301     sa_smartbaud += sa_baudstep * sa_adjdir;
 302
 303     dprintf(("autobaud: %d\r\n", sa_smartbaud));
 304
 305     smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud);
 306
 307     saStat.pktsent = 0;
 308     saStat.pktrcvd = 0;
 309 }
 310
 311 // Transport level variables
 312
 313 static uint32_t sa_lastTransmission = 0;
 314 static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
 315 static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
 316 static int sa_oslen;         // And associate length
 317
 318 #ifdef CMS
 319 void saCmsUpdate(void);
 320 #endif
 321
 322 static void saProcessResponse(uint8_t *buf, int len)
 323 {
 324     uint8_t resp = buf[0];
 325
 326     if (resp == sa_outstanding) {
 327         sa_outstanding = SA_CMD_NONE;
 328     } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) {
 329         sa_outstanding = SA_CMD_NONE;
 330     } else {
 331         saStat.ooopresp++;
 332         dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp));
 333     }
 334
 335     switch(resp) {
 336     case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
 337     case SA_CMD_GET_SETTINGS:    // Version 1 Get Settings
 338         if (len < 7)
 339             break;
 340
 341         saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
 342         saDevice.channel = buf[2];
 343         saDevice.power = buf[3];
 344         saDevice.mode = buf[4];
 345         saDevice.freq = (buf[5] << 8)|buf[6];
 346
 347 #ifdef SMARTAUDIO_DEBUG_MONITOR
 348         debug[0] = saDevice.version * 100 + saDevice.mode;
 349         debug[1] = saDevice.channel;
 350         debug[2] = saDevice.freq;
 351         debug[3] = saDevice.power;
 352 #endif
 353         break;
 354
 355     case SA_CMD_SET_POWER: // Set Power
 356         break;
 357
 358     case SA_CMD_SET_CHAN: // Set Channel
 359         break;
 360
 361     case SA_CMD_SET_FREQ: // Set Frequency
 362         if (len < 5)
 363             break;
 364
 365         uint16_t freq = (buf[2] << 8)|buf[3];
 366
 367         if (freq & SA_FREQ_GETPIT) {
 368             saDevice.orfreq = freq & SA_FREQ_MASK;
 369             dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq));
 370         } else if (freq & SA_FREQ_SETPIT) {
 371             saDevice.orfreq = freq & SA_FREQ_MASK;
 372             dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq));
 373         } else {
 374             saDevice.freq = freq;
 375             dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq));
 376         }
 377         break;
 378
 379     case SA_CMD_SET_MODE: // Set Mode
 380         dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2]));
 381         break;
 382
 383     default:
 384         saStat.badcode++;
 385         return;
 386     }
 387
 388     // Debug
 389     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t)))
 390         saPrintSettings();
 391     saDevicePrev = saDevice;
 392
 393 #ifdef VTX_COMMON
 394     // Todo: Update states in saVtxDevice?
 395 #endif
 396
 397 #ifdef CMS
 398     // Export current device status for CMS
 399     saCmsUpdate();
 400     saUpdateStatusString();
 401 #endif
 402 }
 403
 404 //
 405 // Datalink
 406 //
 407
 408 static void saReceiveFramer(uint8_t c)
 409 {
 410
 411     static enum saFramerState_e {
 412         S_WAITPRE1, // Waiting for preamble 1 (0xAA)
 413         S_WAITPRE2, // Waiting for preamble 2 (0x55)
 414         S_WAITRESP, // Waiting for response code
 415         S_WAITLEN,  // Waiting for length
 416         S_DATA,     // Receiving data
 417         S_WAITCRC,  // Waiting for CRC
 418     } state = S_WAITPRE1;
 419
 420     static int len;
 421     static int dlen;
 422
 423     switch(state) {
 424     case S_WAITPRE1:
 425         if (c == 0xAA) {
 426             state = S_WAITPRE2;
 427         } else {
 428             state = S_WAITPRE1; // Don't need this (no change)
 429         }
 430         break;
 431
 432     case S_WAITPRE2:
 433         if (c == 0x55) {
 434             state = S_WAITRESP;
 435         } else {
 436             saStat.badpre++;
 437             state = S_WAITPRE1;
 438         }
 439         break;
 440
 441     case S_WAITRESP:
 442         sa_rbuf[0] = c;
 443         state = S_WAITLEN;
 444         break;
 445
 446     case S_WAITLEN:
 447         sa_rbuf[1] = c;
 448         len = c;
 449
 450         if (len > SA_MAX_RCVLEN - 2) {
 451             saStat.badlen++;
 452             state = S_WAITPRE1;
 453         } else if (len == 0) {
 454             state = S_WAITCRC;
 455         } else {
 456             dlen = 0;
 457             state = S_DATA;
 458         }
 459         break;
 460
 461     case S_DATA:
 462         // XXX Should check buffer overflow (-> saerr_overflow)
 463         sa_rbuf[2 + dlen] = c;
 464         if (++dlen == len) {
 465             state = S_WAITCRC;
 466         }
 467         break;
 468
 469     case S_WAITCRC:
 470         if (CRC8(sa_rbuf, 2 + len) == c) {
 471             // Got a response
 472             saProcessResponse(sa_rbuf, len + 2);
 473             saStat.pktrcvd++;
 474         } else if (sa_rbuf[0] & 1) {
 475             // Command echo
 476             // XXX There is an exceptional case (V2 response)
 477             // XXX Should check crc in the command format?
 478         } else {
 479             saStat.crc++;
 480         }
 481         state = S_WAITPRE1;
 482         break;
 483     }
 484 }
 485
 486 static void saSendFrame(uint8_t *buf, int len)
 487 {
 488     int i;
 489
 490     serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
 491
 492     for (i = 0 ; i < len ; i++)
 493         serialWrite(smartAudioSerialPort, buf[i]);
 494
 495     serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
 496
 497     sa_lastTransmission = millis();
 498     saStat.pktsent++;
 499 }
 500
 501 /*
 502  * Retransmission and command queuing
 503  *
 504  *   The transport level support includes retransmission on response timeout
 505  * and command queueing.
 506  *
 507  * Resend buffer:
 508  *   The smartaudio returns response for valid command frames in no less
 509  * than 60msec, which we can't wait. So there's a need for a resend buffer.
 510  *
 511  * Command queueing:
 512  *   The driver autonomously sends GetSettings command for auto-bauding,
 513  * asynchronous to user initiated commands; commands issued while another
 514  * command is outstanding must be queued for later processing.
 515  *   The queueing also handles the case in which multiple commands are
 516  * required to implement a user level command.
 517  */
 518
 519 // Retransmission
 520
 521 static void saResendCmd(void)
 522 {
 523     saSendFrame(sa_osbuf, sa_oslen);
 524 }
 525
 526 static void saSendCmd(uint8_t *buf, int len)
 527 {
 528     int i;
 529
 530     for (i = 0 ; i < len ; i++)
 531         sa_osbuf[i] = buf[i];
 532
 533     sa_oslen = len;
 534     sa_outstanding = (buf[2] >> 1);
 535
 536     saSendFrame(sa_osbuf, sa_oslen);
 537 }
 538
 539 // Command queue management
 540
 541 typedef struct saCmdQueue_s {
 542     uint8_t *buf;
 543     int len;
 544 } saCmdQueue_t;
 545
 546 #define SA_QSIZE 4     // 1 heartbeat (GetSettings) + 2 commands + 1 slack
 547 static saCmdQueue_t sa_queue[SA_QSIZE];
 548 static uint8_t sa_qhead = 0;
 549 static uint8_t sa_qtail = 0;
 550
 551 #ifdef DPRINTF_SMARTAUDIO
 552 static int saQueueLength()
 553 {
 554     if (sa_qhead >= sa_qtail) {
 555         return sa_qhead - sa_qtail;
 556     } else {
 557         return SA_QSIZE + sa_qhead - sa_qtail;
 558     }
 559 }
 560 #endif
 561
 562 static bool saQueueEmpty()
 563 {
 564     return sa_qhead == sa_qtail;
 565 }
 566
 567 static bool saQueueFull()
 568 {
 569     return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail;
 570 }
 571
 572 static void saQueueCmd(uint8_t *buf, int len)
 573 {
 574     if (saQueueFull())
 575          return;
 576
 577     sa_queue[sa_qhead].buf = buf;
 578     sa_queue[sa_qhead].len = len;
 579     sa_qhead = (sa_qhead + 1) % SA_QSIZE;
 580 }
 581
 582 static void saSendQueue(void)
 583 {
 584     if (saQueueEmpty())
 585          return;
 586
 587     saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
 588     sa_qtail = (sa_qtail + 1) % SA_QSIZE;
 589 }
 590
 591 // Individual commands
 592
 593 static void saGetSettings(void)
 594 {
 595     static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
 596
 597     saQueueCmd(bufGetSettings, 5);
 598 }
 599
 600 static void saSetFreq(uint16_t freq)
 601 {
 602     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
 603
 604     if (freq & SA_FREQ_GETPIT) {
 605         dprintf(("smartAudioSetFreq: GETPIT\r\n"));
 606     } else if (freq & SA_FREQ_SETPIT) {
 607         dprintf(("smartAudioSetFreq: SETPIT %d\r\n", freq & SA_FREQ_MASK));
 608     } else {
 609         dprintf(("smartAudioSetFreq: SET %d\r\n", freq));
 610     }
 611
 612     buf[4] = (freq >> 8) & 0xff;
 613     buf[5] = freq & 0xff;
 614     buf[6] = CRC8(buf, 6);
 615
 616     saQueueCmd(buf, 7);
 617 }
 618
 619 #if 0
 620 static void saSetPitFreq(uint16_t freq)
 621 {
 622     saSetFreq(freq | SA_FREQ_SETPIT);
 623 }
 624
 625 static void saGetPitFreq(void)
 626 {
 627     saSetFreq(SA_FREQ_GETPIT);
 628 }
 629 #endif
 630
 631 void saSetBandAndChannel(uint8_t band, uint8_t channel)
 632 {
 633     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 634
 635     buf[4] = band * 8 + channel;
 636     buf[5] = CRC8(buf, 5);
 637
 638     saQueueCmd(buf, 6);
 639 }
 640
 641 static void saSetMode(int mode)
 642 {
 643     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
 644
 645     buf[4] = (mode & 0x3f)|saLockMode;
 646     buf[5] = CRC8(buf, 5);
 647
 648     saQueueCmd(buf, 6);
 649 }
 650
 651 void saSetPowerByIndex(uint8_t index)
 652 {
 653     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
 654
 655     dprintf(("saSetPowerByIndex: index %d\r\n", index));
 656
 657     if (saDevice.version == 0) {
 658         // Unknown or yet unknown version.
 659         return;
 660     }
 661
 662     if (index > 3)
 663         return;
 664
 665     buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
 666     buf[5] = CRC8(buf, 5);
 667     saQueueCmd(buf, 6);
 668 }
 669
 670 bool vtxSmartAudioInit()
 671 {
 672 #ifdef SMARTAUDIO_DPRINTF
 673     // Setup debugSerialPort
 674
 675     debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
 676     if (debugSerialPort) {
 677         setPrintfSerialPort(debugSerialPort);
 678         dprintf(("smartAudioInit: OK\r\n"));
 679     }
 680 #endif
 681
 682     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
 683     if (portConfig) {
 684         smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
 685     }
 686
 687     if (!smartAudioSerialPort) {
 688         return false;
 689     }
 690
 691     vtxCommonRegisterDevice(&vtxSmartAudio);
 692
 693     return true;
 694 }
 695
 696 void vtxSAProcess(uint32_t now)
 697 {
 698     static bool initialSent = false;
 699
 700     if (smartAudioSerialPort == NULL)
 701         return;
 702
 703     while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
 704         uint8_t c = serialRead(smartAudioSerialPort);
 705         saReceiveFramer((uint16_t)c);
 706     }
 707
 708     // Re-evaluate baudrate after each frame reception
 709     saAutobaud();
 710
 711     if (!initialSent) {
 712         saGetSettings();
 713         saSetFreq(SA_FREQ_GETPIT);
 714         saSendQueue();
 715         initialSent = true;
 716         return;
 717     }
 718
 719     if ((sa_outstanding != SA_CMD_NONE)
 720             && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
 721         // Last command timed out
 722         // dprintf(("process: resending 0x%x\r\n", sa_outstanding));
 723         // XXX Todo: Resend termination and possible offline transition
 724         saResendCmd();
 725     } else if (!saQueueEmpty()) {
 726         // Command pending. Send it.
 727         // dprintf(("process: sending queue\r\n"));
 728         saSendQueue();
 729     } else if (now - sa_lastTransmission >= 1000) {
 730         // Heart beat for autobauding
 731         //dprintf(("process: sending heartbeat\r\n"));
 732         saGetSettings();
 733         saSendQueue();
 734     }
 735
 736 #ifdef SMARTAUDIO_TEST_VTX_COMMON
 737     // Testing VTX_COMMON API
 738     {
 739         static uint32_t lastMonitorUs = 0;
 740         if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000)
 741             return;
 742
 743         static uint8_t monBand;
 744         static uint8_t monChan;
 745         static uint8_t monPower;
 746
 747         vtxCommonGetBandAndChannel(&monBand, &monChan);
 748         vtxCommonGetPowerIndex(&monPower);
 749         debug[0] = monBand;
 750         debug[1] = monChan;
 751         debug[2] = monPower;
 752     }
 753 #endif
 754 }
 755
 756 #ifdef VTX_COMMON
 757 // Interface to common VTX API
 758
 759 vtxDevType_e vtxSAGetDeviceType(void)
 760 {
 761     return VTXDEV_SMARTAUDIO;
 762 }
 763
 764 bool vtxSAIsReady(void)
 765 {
 766     return !(saDevice.version == 0);
 767 }
 768
 769 void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
 770 {
 771     if (band && channel)
 772         saSetBandAndChannel(band - 1, channel - 1);
 773 }
 774
 775 void vtxSASetPowerByIndex(uint8_t index)
 776 {
 777     if (index == 0) {
 778         // SmartAudio doesn't support power off.
 779         return;
 780     }
 781
 782     saSetPowerByIndex(index - 1);
 783 }
 784
 785 void vtxSASetPitMode(uint8_t onoff)
 786 {
 787     if (!(vtxSAIsReady() && (saDevice.version == 2)))
 788         return;
 789
 790     if (onoff) {
 791         // SmartAudio can not turn pit mode on by software.
 792         return;
 793     }
 794
 795     uint8_t newmode = SA_MODE_CLR_PITMODE;
 796
 797     if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE)
 798         newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
 799
 800     if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
 801         newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
 802
 803     saSetMode(newmode);
 804
 805     return;
 806 }
 807
 808 bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
 809 {
 810     if (!vtxSAIsReady())
 811         return false;
 812
 813     *pBand = (saDevice.channel / 8) + 1;
 814     *pChannel = (saDevice.channel % 8) + 1;
 815     return true;
 816 }
 817
 818 bool vtxSAGetPowerIndex(uint8_t *pIndex)
 819 {
 820     if (!vtxSAIsReady())
 821         return false;
 822
 823     *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
 824     return true;
 825 }
 826
 827 bool vtxSAGetPitMode(uint8_t *pOnOff)
 828 {
 829     if (!(vtxSAIsReady() && (saDevice.version == 2)))
 830         return false;
 831
 832     *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
 833     return true;
 834 }
 835
 836 static const vtxVTable_t saVTable = {
 837     .process = vtxSAProcess,
 838     .getDeviceType = vtxSAGetDeviceType,
 839     .isReady = vtxSAIsReady,
 840     .setBandAndChannel = vtxSASetBandAndChannel,
 841     .setPowerByIndex = vtxSASetPowerByIndex,
 842     .setPitMode = vtxSASetPitMode,
 843     .getBandAndChannel = vtxSAGetBandAndChannel,
 844     .getPowerIndex = vtxSAGetPowerIndex,
 845     .getPitMode = vtxSAGetPitMode,
 846 };
 847 #endif // VTX_COMMON
 848
 849 #ifdef CMS
 850
 851 // Interface to CMS
 852
 853 // Operational Model and RF modes (CMS)
 854
 855 #define SACMS_OPMODEL_UNDEF        0 // Not known yet
 856 #define SACMS_OPMODEL_FREE         1 // Freestyle model: Power up transmitting
 857 #define SACMS_OPMODEL_RACE         2 // Race model: Power up in pit mode
 858
 859 uint8_t  saCmsOpmodel = SACMS_OPMODEL_UNDEF;
 860
 861 #define SACMS_TXMODE_NODEF         0
 862 #define SACMS_TXMODE_PIT_OUTRANGE  1
 863 #define SACMS_TXMODE_PIT_INRANGE   2
 864 #define SACMS_TXMODE_ACTIVE        3
 865
 866 uint8_t  saCmsRFState;          // RF state; ACTIVE, PIR, POR XXX Not currently used
 867
 868 uint8_t  saCmsBand = 0;
 869 uint8_t  saCmsChan = 0;
 870 uint8_t  saCmsPower = 0;
 871
 872 // Frequency derived from channel table (used for reference in band/channel mode)
 873 uint16_t saCmsFreqRef = 0;
 874
 875 uint16_t saCmsDeviceFreq = 0;
 876
 877 uint8_t  saCmsDeviceStatus = 0;
 878 uint8_t  saCmsPower;
 879 uint8_t  saCmsPitFMode;         // In-Range or Out-Range
 880 uint8_t  saCmsFselMode;          // Channel(0) or User defined(1)
 881
 882 uint16_t saCmsORFreq = 0;       // POR frequency
 883 uint16_t saCmsORFreqNew;        // POR frequency
 884
 885 uint16_t saCmsUserFreq = 0;     // User defined frequency
 886 uint16_t saCmsUserFreqNew;      // User defined frequency
 887
 888 void saCmsUpdate(void)
 889 {
 890 // XXX Take care of pit mode update somewhere???
 891
 892     if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
 893         // This is a first valid response to GET_SETTINGS.
 894         saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
 895
 896         saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
 897
 898         saCmsBand = (saDevice.channel / 8) + 1;
 899         saCmsChan = (saDevice.channel % 8) + 1;
 900         saCmsFreqRef = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8];
 901
 902         saCmsDeviceFreq = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8];
 903
 904         if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
 905             saCmsRFState = SACMS_TXMODE_ACTIVE;
 906         } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
 907             saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
 908         } else {
 909             saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
 910         }
 911
 912         if (saDevice.version == 2) {
 913             saCmsPower = saDevice.power + 1; // XXX Take care V1
 914         } else {
 915             saCmsPower = saDacToPowerIndex(saDevice.power) + 1;
 916         }
 917     }
 918
 919     saUpdateStatusString();
 920 }
 921
 922 char saCmsStatusString[31] = "- -- ---- ---";
 923 //                            m bc ffff ppp
 924 //                            0123456789012
 925
 926 static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self);
 927 static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self);
 928 static long saCmsConfigBandByGvar(displayPort_t *, const void *self);
 929 static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
 930 static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);
 931
 932 static void saUpdateStatusString(void)
 933 {
 934     if (saDevice.version == 0)
 935         return;
 936
 937 // XXX These should be done somewhere else
 938 if (saCmsDeviceStatus == 0 && saDevice.version != 0)
 939     saCmsDeviceStatus = saDevice.version;
 940 if (saCmsORFreq == 0 && saDevice.orfreq != 0)
 941     saCmsORFreq = saDevice.orfreq;
 942 if (saCmsUserFreq == 0 && saDevice.freq != 0)
 943     saCmsUserFreq = saDevice.freq;
 944
 945 if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
 946     saCmsPitFMode = 1;
 947 else
 948     saCmsPitFMode = 0;
 949
 950     saCmsStatusString[0] = "-FR"[saCmsOpmodel];
 951
 952     if (saCmsFselMode == 0) {
 953         saCmsStatusString[2] = "ABEFR"[saDevice.channel / 8];
 954         saCmsStatusString[3] = '1' + (saDevice.channel % 8);
 955     } else {
 956         saCmsStatusString[2] = 'U';
 957         saCmsStatusString[3] = 'F';
 958     }
 959
 960     if ((saDevice.mode & SA_MODE_GET_PITMODE)
 961        && (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))
 962         tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.orfreq);
 963     else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ)
 964         tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq);
 965     else
 966         tfp_sprintf(&saCmsStatusString[5], "%4d",
 967             vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]);
 968
 969     saCmsStatusString[9] = ' ';
 970
 971     if (saDevice.mode & SA_MODE_GET_PITMODE) {
 972         saCmsStatusString[10] = 'P';
 973         if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
 974             saCmsStatusString[11] = 'I';
 975         } else {
 976             saCmsStatusString[11] = 'O';
 977         }
 978         saCmsStatusString[12] = 'R';
 979         saCmsStatusString[13] = 0;
 980     } else {
 981         tfp_sprintf(&saCmsStatusString[10], "%3d", (saDevice.version == 2) ?  saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower);
 982     }
 983 }
 984
 985 static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
 986 {
 987     UNUSED(pDisp);
 988     UNUSED(self);
 989
 990     if (saDevice.version == 0) {
 991         // Bounce back; not online yet
 992         saCmsBand = 0;
 993         return 0;
 994     }
 995
 996     if (saCmsBand == 0) {
 997         // Bouce back, no going back to undef state
 998         saCmsBand = 1;
 999         return 0;
1000     }
1001
1002     if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
1003         saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
1004
1005     saCmsFreqRef = vtx58frequencyTable[saCmsBand - 1][saCmsChan - 1];
1006
1007     return 0;
1008 }
1009
1010 static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
1011 {
1012     UNUSED(pDisp);
1013     UNUSED(self);
1014
1015     if (saDevice.version == 0) {
1016         // Bounce back; not online yet
1017         saCmsChan = 0;
1018         return 0;
1019     }
1020
1021     if (saCmsChan == 0) {
1022         // Bounce back; no going back to undef state
1023         saCmsChan = 1;
1024         return 0;
1025     }
1026
1027     if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
1028         saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
1029
1030     saCmsFreqRef = vtx58frequencyTable[saCmsBand - 1][saCmsChan - 1];
1031
1032     return 0;
1033 }
1034
1035 static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
1036 {
1037     UNUSED(pDisp);
1038     UNUSED(self);
1039
1040     if (saDevice.version == 0) {
1041         // Bounce back; not online yet
1042         saCmsPower = 0;
1043         return 0;
1044     }
1045
1046     if (saCmsPower == 0) {
1047         // Bouce back; no going back to undef state
1048         saCmsPower = 1;
1049         return 0;
1050     }
1051
1052     if (saCmsOpmodel == SACMS_OPMODEL_FREE)
1053         saSetPowerByIndex(saCmsPower - 1);
1054
1055     return 0;
1056 }
1057
1058 static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
1059 {
1060     UNUSED(pDisp);
1061     UNUSED(self);
1062
1063     dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode));
1064
1065     if (saCmsPitFMode == 0) {
1066         saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
1067     } else {
1068         saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
1069     }
1070
1071     return 0;
1072 }
1073
1074 static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self); // Forward
1075
1076 static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
1077 {
1078     UNUSED(pDisp);
1079     UNUSED(self);
1080
1081     uint8_t opmodel = saCmsOpmodel;
1082
1083     dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel));
1084
1085     if (opmodel == SACMS_OPMODEL_FREE) {
1086         // VTX should power up transmitting.
1087         // Turn off In-Range and Out-Range bits
1088         saSetMode(0);
1089     } else if (opmodel == SACMS_OPMODEL_RACE) {
1090         // VTX should power up in pit mode.
1091         // Default PitFMode is in-range to prevent users without
1092         // out-range receivers from getting blinded.
1093         saCmsPitFMode = 0;
1094         saCmsConfigPitFModeByGvar(pDisp, self);
1095
1096         // Direct frequency mode is not available in RACE opmodel
1097         saCmsFselMode = 0;
1098         saCmsConfigFreqModeByGvar(pDisp, self);
1099     } else {
1100         // Trying to go back to unknown state; bounce back
1101         saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1;
1102     }
1103
1104     return 0;
1105 }
1106
1107 static const char * const saCmsDeviceStatusNames[] = {
1108     "OFFL",
1109     "ONL V1",
1110     "ONL V2",
1111 };
1112
1113 static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames };
1114
1115 static OSD_Entry saCmsMenuStatsEntries[] = {
1116     { "- SA STATS -", OME_Label, NULL, NULL, 0 },
1117     { "STATUS",   OME_TAB,    NULL, &saCmsEntOnline,                              DYNAMIC },
1118     { "BAUDRATE", OME_UINT16, NULL, &(OSD_UINT16_t){ &sa_smartbaud, 0, 0, 0 },    DYNAMIC },
1119     { "SENT",     OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktsent, 0, 0, 0 },  DYNAMIC },
1120     { "RCVD",     OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktrcvd, 0, 0, 0 },  DYNAMIC },
1121     { "BADPRE",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badpre, 0, 0, 0 },   DYNAMIC },
1122     { "BADLEN",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badlen, 0, 0, 0 },   DYNAMIC },
1123     { "CRCERR",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.crc, 0, 0, 0 },      DYNAMIC },
1124     { "OOOERR",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.ooopresp, 0, 0, 0 }, DYNAMIC },
1125     { "BACK",     OME_Back,   NULL, NULL, 0 },
1126     { NULL,       OME_END,    NULL, NULL, 0 }
1127 };
1128
1129 static CMS_Menu saCmsMenuStats = {
1130     .GUARD_text = "XSAST",
1131     .GUARD_type = OME_MENU,
1132     .onEnter = NULL,
1133     .onExit = NULL,
1134     .onGlobalExit = NULL,
1135     .entries = saCmsMenuStatsEntries
1136 };
1137
1138 static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames };
1139
1140 static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames };
1141
1142 static const char * const saCmsPowerNames[] = {
1143     "---",
1144     "25 ",
1145     "200",
1146     "500",
1147     "800",
1148 };
1149
1150 static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames};
1151
1152 static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 };
1153
1154 static const char * const saCmsOpmodelNames[] = {
1155     "----",
1156     "FREE",
1157     "RACE",
1158 };
1159
1160 static const char * const saCmsFselModeNames[] = {
1161     "CHAN",
1162     "USER"
1163 };
1164
1165 static const char * const saCmsPitFModeNames[] = {
1166     "PIR",
1167     "POR"
1168 };
1169
1170 static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
1171
1172 static long sacms_SetupTopMenu(void); // Forward
1173
1174 static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
1175 {
1176     UNUSED(pDisp);
1177     UNUSED(self);
1178
1179     if (saCmsFselMode == 0) {
1180         // CHAN
1181         saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
1182     } else {
1183         // USER: User frequency mode is only available in FREE opmodel.
1184         if (saCmsOpmodel == SACMS_OPMODEL_FREE) {
1185             saSetFreq(saCmsUserFreq);
1186         } else {
1187             // Bounce back
1188             saCmsFselMode = 0;
1189         }
1190     }
1191
1192     sacms_SetupTopMenu();
1193
1194     return 0;
1195 }
1196
1197 static long saCmsCommence(displayPort_t *pDisp, const void *self)
1198 {
1199     UNUSED(pDisp);
1200     UNUSED(self);
1201
1202     if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
1203         // Race model
1204         // Setup band, freq and power.
1205
1206         saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
1207
1208         // If in pit mode, cancel it.
1209
1210         if (saCmsPitFMode == 0)
1211             saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
1212         else
1213             saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
1214     } else {
1215         // Freestyle model
1216         // Setup band and freq / user freq
1217         if (saCmsFselMode == 0)
1218             saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
1219         else
1220             saSetFreq(saCmsUserFreq);
1221     }
1222
1223     saSetPowerByIndex(saCmsPower - 1);
1224
1225     return MENU_CHAIN_BACK;
1226 }
1227
1228 static long saCmsSetPORFreqOnEnter(void)
1229 {
1230     saCmsORFreqNew = saCmsORFreq;
1231
1232     return 0;
1233 }
1234
1235 static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
1236 {
1237     UNUSED(pDisp);
1238     UNUSED(self);
1239
1240     saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT);
1241
1242     return 0;
1243 }
1244
1245 static char *saCmsORFreqGetString(void)
1246 {
1247     static char pbuf[5];
1248
1249     tfp_sprintf(pbuf, "%4d", saCmsORFreq);
1250
1251     return pbuf;
1252 }
1253
1254 static char *saCmsUserFreqGetString(void)
1255 {
1256     static char pbuf[5];
1257
1258     tfp_sprintf(pbuf, "%4d", saCmsUserFreq);
1259
1260     return pbuf;
1261 }
1262
1263 static long saCmsSetUserFreqOnEnter(void)
1264 {
1265     saCmsUserFreqNew = saCmsUserFreq;
1266
1267     return 0;
1268 }
1269
1270 static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self)
1271 {
1272     UNUSED(pDisp);
1273     UNUSED(self);
1274
1275     saCmsUserFreq = saCmsUserFreqNew;
1276     //saSetFreq(saCmsUserFreq);
1277
1278     return MENU_CHAIN_BACK;
1279 }
1280
1281 static OSD_Entry saCmsMenuPORFreqEntries[] = {
1282     { "- POR FREQ -", OME_Label,   NULL,             NULL,                                                 0 },
1283
1284     { "CUR FREQ",     OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsORFreq, 5000, 5900, 0 },       DYNAMIC },
1285     { "NEW FREQ",     OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 },    0 },
1286     { "SET",          OME_Funcall, saCmsSetPORFreq,  NULL,                                                 0 },
1287
1288     { "BACK",         OME_Back,    NULL,             NULL,                                                 0 },
1289     { NULL,           OME_END,     NULL,             NULL,                                                 0 }
1290 };
1291
1292 static CMS_Menu saCmsMenuPORFreq =
1293 {
1294     .GUARD_text = "XSAPOR",
1295     .GUARD_type = OME_MENU,
1296     .onEnter = saCmsSetPORFreqOnEnter,
1297     .onExit = NULL,
1298     .onGlobalExit = NULL,
1299     .entries = saCmsMenuPORFreqEntries,
1300 };
1301
1302 static OSD_Entry saCmsMenuUserFreqEntries[] = {
1303     { "- USER FREQ -", OME_Label,   NULL,             NULL,                                                0 },
1304
1305     { "CUR FREQ",      OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsUserFreq, 5000, 5900, 0 },    DYNAMIC },
1306     { "NEW FREQ",      OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 }, 0 },
1307     { "SET",           OME_Funcall, saCmsConfigUserFreq, NULL,                                                0 },
1308
1309     { "BACK",          OME_Back,    NULL,             NULL,                                                0 },
1310     { NULL,            OME_END,     NULL,             NULL,                                                0 }
1311 };
1312
1313 static CMS_Menu saCmsMenuUserFreq =
1314 {
1315     .GUARD_text = "XSAUFQ",
1316     .GUARD_type = OME_MENU,
1317     .onEnter = saCmsSetUserFreqOnEnter,
1318     .onExit = NULL,
1319     .onGlobalExit = NULL,
1320     .entries = saCmsMenuUserFreqEntries,
1321 };
1322
1323 static OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames };
1324
1325 static OSD_Entry saCmsMenuConfigEntries[] = {
1326     { "- SA CONFIG -", OME_Label, NULL, NULL, 0 },
1327
1328     { "OP MODEL",  OME_TAB,     saCmsConfigOpmodelByGvar,              &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, DYNAMIC },
1329     { "FSEL MODE", OME_TAB,     saCmsConfigFreqModeByGvar,             &saCmsEntFselMode,                                   DYNAMIC },
1330     { "PIT FMODE", OME_TAB,     saCmsConfigPitFModeByGvar,             &saCmsEntPitFMode,                                   0 },
1331     { "POR FREQ",  OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq,                                   OPTSTRING },
1332     { "STATX",     OME_Submenu, cmsMenuChange,                         &saCmsMenuStats,                                     0 },
1333
1334     { "BACK", OME_Back, NULL, NULL, 0 },
1335     { NULL, OME_END, NULL, NULL, 0 }
1336 };
1337
1338 static CMS_Menu saCmsMenuConfig = {
1339     .GUARD_text = "XSACFG",
1340     .GUARD_type = OME_MENU,
1341     .onEnter = NULL,
1342     .onExit = NULL,
1343     .onGlobalExit = NULL,
1344     .entries = saCmsMenuConfigEntries
1345 };
1346
1347 static OSD_Entry saCmsMenuCommenceEntries[] = {
1348     { "CONFIRM", OME_Label,   NULL,          NULL, 0 },
1349
1350     { "YES",     OME_Funcall, saCmsCommence, NULL, 0 },
1351
1352     { "BACK",    OME_Back, NULL, NULL, 0 },
1353     { NULL,      OME_END, NULL, NULL, 0 }
1354 };
1355
1356 static CMS_Menu saCmsMenuCommence = {
1357     .GUARD_text = "XVTXCOM",
1358     .GUARD_type = OME_MENU,
1359     .onEnter = NULL,
1360     .onExit = NULL,
1361     .onGlobalExit = NULL,
1362     .entries = saCmsMenuCommenceEntries,
1363 };
1364
1365 static OSD_Entry saCmsMenuFreqModeEntries[] = {
1366     { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
1367
1368     { "",       OME_Label,   NULL,                                     saCmsStatusString,  DYNAMIC },
1369     { "FREQ",   OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString,  &saCmsMenuUserFreq, OPTSTRING },
1370     { "POWER",  OME_TAB,     saCmsConfigPowerByGvar,                   &saCmsEntPower,     0 },
1371     { "SET",    OME_Submenu, cmsMenuChange,                            &saCmsMenuCommence, 0 },
1372     { "CONFIG", OME_Submenu, cmsMenuChange,                            &saCmsMenuConfig,   0 },
1373
1374     { "BACK", OME_Back, NULL, NULL, 0 },
1375     { NULL, OME_END, NULL, NULL, 0 }
1376 };
1377
1378 static OSD_Entry saCmsMenuChanModeEntries[] =
1379 {
1380     { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
1381
1382     { "",       OME_Label,   NULL,                   saCmsStatusString,  DYNAMIC },
1383     { "BAND",   OME_TAB,     saCmsConfigBandByGvar,  &saCmsEntBand,      0 },
1384     { "CHAN",   OME_TAB,     saCmsConfigChanByGvar,  &saCmsEntChan,      0 },
1385     { "(FREQ)", OME_UINT16,  NULL,                   &saCmsEntFreqRef,   DYNAMIC },
1386     { "POWER",  OME_TAB,     saCmsConfigPowerByGvar, &saCmsEntPower,     0 },
1387     { "SET",    OME_Submenu, cmsMenuChange,          &saCmsMenuCommence, 0 },
1388     { "CONFIG", OME_Submenu, cmsMenuChange,          &saCmsMenuConfig,   0 },
1389
1390     { "BACK",   OME_Back, NULL, NULL, 0 },
1391     { NULL,     OME_END, NULL, NULL, 0 }
1392 };
1393
1394 static OSD_Entry saCmsMenuOfflineEntries[] =
1395 {
1396     { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
1397
1398     { "",      OME_Label,   NULL,          saCmsStatusString, DYNAMIC },
1399     { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats,   0 },
1400
1401     { "BACK",  OME_Back, NULL, NULL, 0 },
1402     { NULL,    OME_END, NULL, NULL, 0 }
1403 };
1404
1405 CMS_Menu cmsx_menuVtxSmartAudio; // Forward
1406
1407 static long sacms_SetupTopMenu(void)
1408 {
1409     if (saCmsDeviceStatus) {
1410         if (saCmsFselMode == 0)
1411             cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
1412         else
1413             cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
1414     } else {
1415         cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries;
1416     }
1417
1418     return 0;
1419 }
1420
1421 CMS_Menu cmsx_menuVtxSmartAudio = {
1422     .GUARD_text = "XVTXSA",
1423     .GUARD_type = OME_MENU,
1424     .onEnter = sacms_SetupTopMenu,
1425     .onExit = NULL,
1426     .onGlobalExit = NULL,
1427     .entries = saCmsMenuOfflineEntries,
1428 };
1429
1430 #endif // CMS
1431
1432 #endif // VTX_SMARTAUDIO