diff --git a/Makefile b/Makefile
index c7f86dfdae..129b4e0c94 100644
--- a/Makefile
+++ b/Makefile
@@ -591,6 +591,7 @@ HIGHEND_SRC = \
drivers/serial_escserial.c \
drivers/serial_softserial.c \
drivers/sonar_hcsr04.c \
+ drivers/vtx_common.c \
flight/navigation.c \
flight/gps_conversion.c \
io/dashboard.c \
@@ -612,7 +613,7 @@ HIGHEND_SRC = \
telemetry/mavlink.c \
telemetry/ibus.c \
sensors/esc_sensor.c \
- io/vtx_common.c \
+ io/vtx_string.c \
io/vtx_smartaudio.c \
io/vtx_tramp.c
@@ -706,6 +707,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/serial_escserial.c \
+ drivers/vtx_common.c \
io/cli.c \
io/serial_4way.c \
io/serial_4way_avrootloader.c \
diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c
new file mode 100644
index 0000000000..7e617b03ac
--- /dev/null
+++ b/src/main/drivers/vtx_common.c
@@ -0,0 +1,124 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/* Created by jflyper */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+#include "build/debug.h"
+
+#if defined(VTX_COMMON)
+
+#include "vtx_common.h"
+
+vtxDevice_t *vtxDevice = NULL;
+
+void vtxCommonInit(void)
+{
+}
+
+// Whatever registered last will win
+
+void vtxCommonRegisterDevice(vtxDevice_t *pDevice)
+{
+ vtxDevice = pDevice;
+}
+
+void vtxCommonProcess(uint32_t currentTimeUs)
+{
+ if (!vtxDevice)
+ return;
+
+ if (vtxDevice->vTable->process)
+ vtxDevice->vTable->process(currentTimeUs);
+}
+
+vtxDevType_e vtxCommonGetDeviceType(void)
+{
+ if (!vtxDevice)
+ return VTXDEV_UNKNOWN;
+
+ return vtxDevice->devtype;
+}
+
+// band and chan are 1 origin
+void vtxCommonSetBandChan(uint8_t band, uint8_t chan)
+{
+ if (!vtxDevice)
+ return;
+
+ if (vtxDevice->vTable->setBandChan)
+ vtxDevice->vTable->setBandChan(band, chan);
+}
+
+// index is zero origin, zero = power off completely
+void vtxCommonSetPowerByIndex(uint8_t index)
+{
+ if (!vtxDevice)
+ return;
+
+ if (vtxDevice->vTable->setPowerByIndex)
+ vtxDevice->vTable->setPowerByIndex(index);
+}
+
+// on = 1, off = 0
+void vtxCommonSetPitmode(uint8_t onoff)
+{
+ if (!vtxDevice)
+ return;
+
+ if (vtxDevice->vTable->setPitmode)
+ vtxDevice->vTable->setPitmode(onoff);
+}
+
+bool vtxCommonGetBandChan(uint8_t *pBand, uint8_t *pChan)
+{
+ if (!vtxDevice)
+ return false;
+
+ if (vtxDevice->vTable->getBandChan)
+ return vtxDevice->vTable->getBandChan(pBand, pChan);
+ else
+ return false;
+}
+
+bool vtxCommonGetPowerIndex(uint8_t *pIndex)
+{
+ if (!vtxDevice)
+ return false;
+
+ if (vtxDevice->vTable->getPowerIndex)
+ return vtxDevice->vTable->getPowerIndex(pIndex);
+ else
+ return false;
+}
+
+bool vtxCommonGetPitmode(uint8_t *pOnoff)
+{
+ if (!vtxDevice)
+ return false;
+
+ if (vtxDevice->vTable->getPitmode)
+ return vtxDevice->vTable->getPitmode(pOnoff);
+ else
+ return false;
+}
+#endif
diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h
new file mode 100644
index 0000000000..c7c521c666
--- /dev/null
+++ b/src/main/drivers/vtx_common.h
@@ -0,0 +1,83 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/* Created by jflyper */
+
+typedef enum {
+ VTXDEV_UNKNOWN = 0,
+ VTXDEV_SMARTAUDIO = 3,
+ VTXDEV_TRAMP = 4,
+} vtxDevType_e;
+
+struct vtxVTable_s;
+
+typedef struct vtxDevice_s {
+ const struct vtxVTable_s *vTable;
+
+ vtxDevType_e devtype; // 3.1 only; eventually goes away
+
+ uint8_t numBand;
+ uint8_t numChan;
+ uint8_t numPower;
+
+ uint16_t *freqTable; // Array of [numBand][numChan]
+ char **bandNames; // char *bandNames[numBand]
+ char **chanNames; // char *chanNames[numChan]
+ char **powerNames; // char *powerNames[numPower]
+
+ uint8_t curBand;
+ uint8_t curChan;
+ uint8_t curPowerIndex;
+ uint8_t curPitState; // 0 = non-PIT, 1 = PIT
+
+} vtxDevice_t;
+
+// {set,get}BandChan: band and chan are 1 origin
+// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
+// {set,get}Pitmode: 0 = OFF, 1 = ON
+
+typedef struct vtxVTable_s {
+ void (*process)(uint32_t currentTimeUs);
+ vtxDevType_e (*getDeviceType)(void);
+ bool (*isReady)(void);
+
+ void (*setBandChan)(uint8_t band, uint8_t chan);
+ void (*setPowerByIndex)(uint8_t level);
+ void (*setPitmode)(uint8_t onoff);
+
+ bool (*getBandChan)(uint8_t *pBand, uint8_t *pChan);
+ bool (*getPowerIndex)(uint8_t *pIndex);
+ bool (*getPitmode)(uint8_t *pOnoff);
+} vtxVTable_t;
+
+// 3.1.0
+// PIT mode is defined as LOWEST POSSIBLE RF POWER.
+// - It can be a dedicated mode, or lowest RF power possible.
+// - It is *NOT* RF on/off control ?
+
+void vtxCommonInit(void);
+void vtxCommonRegisterDevice(vtxDevice_t *pDevice);
+
+// VTable functions
+void vtxCommonProcess(uint32_t currentTimeUs);
+uint8_t vtxCommonGetDeviceType(void);
+void vtxCommonSetBandChan(uint8_t band, uint8_t chan);
+void vtxCommonSetPowerByIndex(uint8_t level);
+void vtxCommonSetPitmode(uint8_t onoff);
+bool vtxCommonGetBandChan(uint8_t *pBand, uint8_t *pChan);
+bool vtxCommonGetPowerIndex(uint8_t *pIndex);
+bool vtxCommonGetPitmode(uint8_t *pOnoff);
diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c
index 38915a8750..ea9403b87e 100644
--- a/src/main/drivers/vtx_rtc6705.c
+++ b/src/main/drivers/vtx_rtc6705.c
@@ -87,7 +87,9 @@
#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
+#if defined(SPRACINGF3NEO)
static IO_t vtxPowerPin = IO_NONE;
+#endif
#define ENABLE_VTX_POWER IOLo(vtxPowerPin)
#define DISABLE_VTX_POWER IOHi(vtxPowerPin)
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index b746f0252e..2a4d25c5f1 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -32,6 +32,7 @@
#include "drivers/compass.h"
#include "drivers/serial.h"
#include "drivers/stack_check.h"
+#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "fc/fc_msp.h"
@@ -52,8 +53,7 @@
#include "io/osd.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
-#include "io/vtx_smartaudio.h"
-#include "io/vtx_tramp.h"
+#include "io/vtx_tramp.h" // Will be gone
#include "msp/msp_serial.h"
@@ -213,9 +213,10 @@ void taskVtxControl(uint32_t currentTime)
if (ARMING_FLAG(ARMED))
return;
-#ifdef VTX_SMARTAUDIO
- smartAudioProcess(currentTime);
+#ifdef VTX_COMMON
+ vtxCommonProcess(currentTime);
#endif
+// Call to trampProcess() will be gone
#ifdef VTX_TRAMP
trampProcess(currentTime);
#endif
diff --git a/src/main/fc/fc_tasks.c.orig b/src/main/fc/fc_tasks.c.orig
new file mode 100644
index 0000000000..8e9e5bf31d
--- /dev/null
+++ b/src/main/fc/fc_tasks.c.orig
@@ -0,0 +1,517 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+
+#include
+
+#include "cms/cms.h"
+
+#include "common/axis.h"
+#include "common/color.h"
+#include "common/utils.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/compass.h"
+#include "drivers/serial.h"
+#include "drivers/stack_check.h"
+#include "drivers/vtx_common.h"
+
+#include "fc/config.h"
+#include "fc/fc_msp.h"
+#include "fc/fc_tasks.h"
+#include "fc/fc_core.h"
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+#include "fc/cli.h"
+#include "fc/fc_dispatch.h"
+
+#include "flight/pid.h"
+#include "flight/altitudehold.h"
+
+#include "io/beeper.h"
+#include "io/dashboard.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/osd.h"
+#include "io/serial.h"
+#include "io/transponder_ir.h"
+<<<<<<< HEAD
+=======
+#include "io/vtx_smartaudio.h"
+#include "io/vtx_tramp.h"
+>>>>>>> betaflight/master
+
+#include "msp/msp_serial.h"
+
+#include "rx/rx.h"
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sonar.h"
+#include "sensors/esc_sensor.h"
+
+#include "scheduler/scheduler.h"
+
+#include "telemetry/telemetry.h"
+
+#include "config/feature.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+#ifdef USE_BST
+void taskBstMasterProcess(timeUs_t currentTimeUs);
+#endif
+
+#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
+#define TASK_PERIOD_MS(ms) ((ms) * 1000)
+#define TASK_PERIOD_US(us) (us)
+
+/* VBAT monitoring interval (in microseconds) - 1s*/
+#define VBATINTERVAL (6 * 3500)
+/* IBat monitoring interval (in microseconds) - 6 default looptimes */
+#define IBATINTERVAL (6 * 3500)
+
+
+static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ accUpdate(&accelerometerConfig()->accelerometerTrims);
+}
+
+static void taskHandleSerial(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+#ifdef USE_CLI
+ // in cli mode, all serial stuff goes to here. enter cli mode by sending #
+ if (cliMode) {
+ cliProcess();
+ return;
+ }
+#endif
+ mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
+}
+
+static void taskUpdateBattery(timeUs_t currentTimeUs)
+{
+#if defined(USE_ADC) || defined(USE_ESC_SENSOR)
+ if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) {
+ static uint32_t vbatLastServiced = 0;
+ if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
+ vbatLastServiced = currentTimeUs;
+ updateBattery();
+ }
+ }
+#endif
+
+ if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
+ static uint32_t ibatLastServiced = 0;
+ const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
+
+ if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
+ ibatLastServiced = currentTimeUs;
+ updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
+ }
+ }
+}
+
+static void taskUpdateRxMain(timeUs_t currentTimeUs)
+{
+ processRx(currentTimeUs);
+ isRXDataNew = true;
+
+#if !defined(BARO) && !defined(SONAR)
+ // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
+ updateRcCommands();
+#endif
+ updateLEDs();
+
+#ifdef BARO
+ if (sensors(SENSOR_BARO)) {
+ updateAltHoldState();
+ }
+#endif
+
+#ifdef SONAR
+ if (sensors(SENSOR_SONAR)) {
+ updateSonarAltHoldState();
+ }
+#endif
+}
+
+#ifdef MAG
+static void taskUpdateCompass(timeUs_t currentTimeUs)
+{
+ if (sensors(SENSOR_MAG)) {
+ compassUpdate(currentTimeUs, &compassConfig()->magZero);
+ }
+}
+#endif
+
+#ifdef BARO
+static void taskUpdateBaro(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ if (sensors(SENSOR_BARO)) {
+ const uint32_t newDeadline = baroUpdate();
+ if (newDeadline != 0) {
+ rescheduleTask(TASK_SELF, newDeadline);
+ }
+ }
+}
+#endif
+
+#if defined(BARO) || defined(SONAR)
+static void taskCalculateAltitude(timeUs_t currentTimeUs)
+{
+ if (false
+#if defined(BARO)
+ || (sensors(SENSOR_BARO) && isBaroReady())
+#endif
+#if defined(SONAR)
+ || sensors(SENSOR_SONAR)
+#endif
+ ) {
+ calculateEstimatedAltitude(currentTimeUs);
+ }}
+#endif
+
+#ifdef TELEMETRY
+static void taskTelemetry(timeUs_t currentTimeUs)
+{
+ telemetryCheckState();
+
+ if (!cliMode && feature(FEATURE_TELEMETRY)) {
+ telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
+ }
+}
+#endif
+
+#ifdef VTX_CONTROL
+// Everything that listens to VTX devices
+void taskVtxControl(uint32_t currentTime)
+{
+ if (ARMING_FLAG(ARMED))
+ return;
+
+#ifdef VTX_COMMON
+ vtxCommonProcess(currentTime);
+#endif
+#ifdef VTX_TRAMP
+ trampProcess(currentTime);
+#endif
+}
+#endif
+
+void fcTasksInit(void)
+{
+ schedulerInit();
+ rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
+ setTaskEnabled(TASK_GYROPID, true);
+
+ if (sensors(SENSOR_ACC)) {
+ setTaskEnabled(TASK_ACCEL, true);
+ rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
+ }
+
+ setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
+ setTaskEnabled(TASK_SERIAL, true);
+ rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
+ setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
+ setTaskEnabled(TASK_RX, true);
+
+ setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());
+
+#ifdef BEEPER
+ setTaskEnabled(TASK_BEEPER, true);
+#endif
+#ifdef GPS
+ setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
+#endif
+#ifdef MAG
+ setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
+#if defined(USE_SPI) && defined(USE_MAG_AK8963)
+ // fixme temporary solution for AK6983 via slave I2C on MPU9250
+ rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
+#endif
+#endif
+#ifdef BARO
+ setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
+#endif
+#ifdef SONAR
+ setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
+#endif
+#if defined(BARO) || defined(SONAR)
+ setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
+#endif
+#ifdef USE_DASHBOARD
+ setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
+#endif
+#ifdef TELEMETRY
+ setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
+ if (feature(FEATURE_TELEMETRY)) {
+ if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) {
+ // Reschedule telemetry to 500hz for Jeti Exbus
+ rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+ } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
+ // Reschedule telemetry to 500hz, 2ms for CRSF
+ rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+ }
+ }
+#endif
+#ifdef LED_STRIP
+ setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
+#endif
+#ifdef TRANSPONDER
+ setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
+#endif
+#ifdef OSD
+ setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
+#endif
+#ifdef USE_BST
+ setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
+#endif
+#ifdef USE_ESC_SENSOR
+ setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR));
+#endif
+#ifdef CMS
+#ifdef USE_MSP_DISPLAYPORT
+ setTaskEnabled(TASK_CMS, true);
+#else
+ setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
+#endif
+#endif
+#ifdef STACK_CHECK
+ setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
+#ifdef VTX_CONTROL
+#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
+ setTaskEnabled(TASK_VTXCTRL, true);
+#endif
+#endif
+}
+
+cfTask_t cfTasks[TASK_COUNT] = {
+ [TASK_SYSTEM] = {
+ .taskName = "SYSTEM",
+ .taskFunc = taskSystem,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms
+ .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
+ },
+
+ [TASK_GYROPID] = {
+ .taskName = "PID",
+ .subTaskName = "GYRO",
+ .taskFunc = taskMainPidLoop,
+ .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
+ .staticPriority = TASK_PRIORITY_REALTIME,
+ },
+
+ [TASK_ACCEL] = {
+ .taskName = "ACCEL",
+ .taskFunc = taskUpdateAccelerometer,
+ .desiredPeriod = TASK_PERIOD_HZ(1000), // 1000Hz, every 1ms
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+
+ [TASK_ATTITUDE] = {
+ .taskName = "ATTITUDE",
+ .taskFunc = imuUpdateAttitude,
+ .desiredPeriod = TASK_PERIOD_HZ(100),
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+
+ [TASK_RX] = {
+ .taskName = "RX",
+ .checkFunc = rxUpdateCheck,
+ .taskFunc = taskUpdateRxMain,
+ .desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling
+ .staticPriority = TASK_PRIORITY_HIGH,
+ },
+
+ [TASK_SERIAL] = {
+ .taskName = "SERIAL",
+ .taskFunc = taskHandleSerial,
+ .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+
+ [TASK_DISPATCH] = {
+ .taskName = "DISPATCH",
+ .taskFunc = dispatchProcess,
+ .desiredPeriod = TASK_PERIOD_HZ(1000),
+ .staticPriority = TASK_PRIORITY_HIGH,
+ },
+
+ [TASK_BATTERY] = {
+ .taskName = "BATTERY",
+ .taskFunc = taskUpdateBattery,
+ .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+
+#ifdef BEEPER
+ [TASK_BEEPER] = {
+ .taskName = "BEEPER",
+ .taskFunc = beeperUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef GPS
+ [TASK_GPS] = {
+ .taskName = "GPS",
+ .taskFunc = gpsUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // GPS usually don't go raster than 10Hz
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+#endif
+
+#ifdef MAG
+ [TASK_COMPASS] = {
+ .taskName = "COMPASS",
+ .taskFunc = taskUpdateCompass,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef BARO
+ [TASK_BARO] = {
+ .taskName = "BARO",
+ .taskFunc = taskUpdateBaro,
+ .desiredPeriod = TASK_PERIOD_HZ(20),
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef SONAR
+ [TASK_SONAR] = {
+ .taskName = "SONAR",
+ .taskFunc = sonarUpdate,
+ .desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfer with each other
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#if defined(BARO) || defined(SONAR)
+ [TASK_ALTITUDE] = {
+ .taskName = "ALTITUDE",
+ .taskFunc = taskCalculateAltitude,
+ .desiredPeriod = TASK_PERIOD_HZ(40),
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef TRANSPONDER
+ [TASK_TRANSPONDER] = {
+ .taskName = "TRANSPONDER",
+ .taskFunc = transponderUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef USE_DASHBOARD
+ [TASK_DASHBOARD] = {
+ .taskName = "DASHBOARD",
+ .taskFunc = dashboardUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(10),
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+#ifdef OSD
+ [TASK_OSD] = {
+ .taskName = "OSD",
+ .taskFunc = osdUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+#ifdef TELEMETRY
+ [TASK_TELEMETRY] = {
+ .taskName = "TELEMETRY",
+ .taskFunc = taskTelemetry,
+ .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef LED_STRIP
+ [TASK_LEDSTRIP] = {
+ .taskName = "LEDSTRIP",
+ .taskFunc = ledStripUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef USE_BST
+ [TASK_BST_MASTER_PROCESS] = {
+ .taskName = "BST_MASTER_PROCESS",
+ .taskFunc = taskBstMasterProcess,
+ .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz, 20ms
+ .staticPriority = TASK_PRIORITY_IDLE,
+ },
+#endif
+
+#ifdef USE_ESC_SENSOR
+ [TASK_ESC_SENSOR] = {
+ .taskName = "ESC_SENSOR",
+ .taskFunc = escSensorProcess,
+ .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz every 10ms
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef CMS
+ [TASK_CMS] = {
+ .taskName = "CMS",
+ .taskFunc = cmsHandler,
+ .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+#endif
+
+#ifdef STACK_CHECK
+ [TASK_STACK_CHECK] = {
+ .taskName = "STACKCHECK",
+ .taskFunc = taskStackCheck,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz
+ .staticPriority = TASK_PRIORITY_IDLE,
+ },
+#endif
+
+#ifdef VTX_CONTROL
+ [TASK_VTXCTRL] = {
+ .taskName = "VTXCTRL",
+ .taskFunc = taskVtxControl,
+ .desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec
+ .staticPriority = TASK_PRIORITY_IDLE,
+ },
+#endif
+};
diff --git a/src/main/io/vtx_singularity.c b/src/main/io/vtx_singularity.c
new file mode 100644
index 0000000000..1831a6c816
--- /dev/null
+++ b/src/main/io/vtx_singularity.c
@@ -0,0 +1,110 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+
+// Get target build configuration
+#include "platform.h"
+
+#ifdef VTX_SINGULARITY
+
+// Own interfaces
+#include "io/vtx_singularity.h"
+#include "io/osd.h"
+
+//External dependencies
+#include "config/config_master.h"
+#include "config/config_eeprom.h"
+#include "drivers/vtx_rtc6705.h"
+#include "fc/runtime_config.h"
+#include "io/beeper.h"
+
+
+static uint8_t locked = 0;
+
+void vtxSingularityInit(void)
+{
+ rtc6705Init();
+ if (masterConfig.vtx_mode == 0) {
+ rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel);
+ } else if (masterConfig.vtx_mode == 1) {
+ rtc6705SetFreq(masterConfig.vtx_mhz);
+ }
+}
+
+static void setChannelSaveAndNotify(uint8_t *bandOrChannel, uint8_t step, int32_t min, int32_t max)
+{
+ if (ARMING_FLAG(ARMED)) {
+ locked = 1;
+ }
+
+ if (masterConfig.vtx_mode == 0 && !locked) {
+ uint8_t temp = (*bandOrChannel) + step;
+ temp = constrain(temp, min, max);
+ *bandOrChannel = temp;
+
+ rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel);
+ writeEEPROM();
+ readEEPROM();
+ beeperConfirmationBeeps(temp);
+ }
+}
+
+void vtxSingularityIncrementBand(void)
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
+}
+
+void vtxSingularityDecrementBand(void)
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
+}
+
+void vtxSingularityIncrementChannel(void)
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
+}
+
+void vtxSingularityDecrementChannel(void)
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
+}
+
+void vtxSingularityUpdateActivatedChannel(void)
+{
+ if (ARMING_FLAG(ARMED)) {
+ locked = 1;
+ }
+
+ if (masterConfig.vtx_mode == 2 && !locked) {
+ static uint8_t lastIndex = -1;
+ uint8_t index;
+
+ for (index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) {
+ vtxChannelActivationCondition_t *vtxChannelActivationCondition = &masterConfig.vtxChannelActivationConditions[index];
+
+ if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range)
+ && index != lastIndex) {
+ lastIndex = index;
+ rtc6705SetChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel);
+ break;
+ }
+ }
+ }
+}
+
+#endif
+
diff --git a/src/main/io/vtx_singularity.h b/src/main/io/vtx_singularity.h
new file mode 100644
index 0000000000..a823aacb82
--- /dev/null
+++ b/src/main/io/vtx_singularity.h
@@ -0,0 +1,41 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include "fc/rc_controls.h"
+
+#define VTX_BAND_MIN 1
+#define VTX_BAND_MAX 5
+#define VTX_CHANNEL_MIN 1
+#define VTX_CHANNEL_MAX 8
+#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10
+
+typedef struct vtxChannelActivationCondition_s {
+ uint8_t auxChannelIndex;
+ uint8_t band;
+ uint8_t channel;
+ channelRange_t range;
+} vtxChannelActivationCondition_t;
+
+void vtxSingularityInit(void);
+void vtxSingularityIncrementBand(void);
+void vtxSingularityDecrementBand(void);
+void vtxSingularityIncrementChannel(void);
+void vtxSingularityDecrementChannel(void);
+void vtxSingularityUpdateActivatedChannel(void);
+
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 19b9cad91c..29811bbfcc 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -33,9 +33,10 @@
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
+#include "drivers/vtx_common.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
-#include "io/vtx_common.h"
+#include "io/vtx_string.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@@ -70,6 +71,25 @@ static void saUpdateStatusString(void); // Forward
static serialPort_t *smartAudioSerialPort = NULL;
+#if defined(CMS) || defined(VTX_COMMON)
+static const char * const saPowerNames[] = {
+ "---", "25 ", "200", "500", "800",
+};
+#endif
+
+#ifdef VTX_COMMON
+static vtxVTable_t saVTable; // Forward
+static vtxDevice_t vtxSmartAudio = {
+ .vTable = &saVTable,
+ .numBand = 5,
+ .numChan = 8,
+ .numPower = 4,
+ .bandNames = (char **)vtx58BandNames,
+ .chanNames = (char **)vtx58ChannelNames,
+ .powerNames = (char **)saPowerNames,
+};
+#endif
+
// SmartAudio command and response codes
enum {
SA_CMD_NONE = 0x00,
@@ -228,11 +248,11 @@ static int saDacToPowerIndex(int dac)
{
int idx;
- for (idx = 0 ; idx < 4 ; idx++) {
+ for (idx = 3 ; idx >= 0 ; idx--) {
if (saPowerTable[idx].valueV1 <= dac)
return(idx);
}
- return(3);
+ return(0);
}
//
@@ -367,6 +387,10 @@ static void saProcessResponse(uint8_t *buf, int len)
saPrintSettings();
saDevicePrev = saDevice;
+#ifdef VTX_COMMON
+ // Todo: Update states in saVtxDevice?
+#endif
+
#ifdef CMS
// Export current device status for CMS
saCmsUpdate();
@@ -661,10 +685,13 @@ bool smartAudioInit()
return false;
}
+ vtxSmartAudio.vTable = &saVTable;
+ vtxCommonRegisterDevice(&vtxSmartAudio);
+
return true;
}
-void smartAudioProcess(uint32_t now)
+void vtxSAProcess(uint32_t now)
{
static bool initialSent = false;
@@ -703,8 +730,120 @@ void smartAudioProcess(uint32_t now)
saGetSettings();
saSendQueue();
}
+
+#ifdef SMARTAUDIO_TEST_VTX_COMMON
+ // Testing VTX_COMMON API
+ {
+ static uint32_t lastMonitorUs = 0;
+ if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000)
+ return;
+
+ static uint8_t monBand;
+ static uint8_t monChan;
+ static uint8_t monPower;
+
+ vtxCommonGetBandChan(&monBand, &monChan);
+ vtxCommonGetPowerIndex(&monPower);
+ debug[0] = monBand;
+ debug[1] = monChan;
+ debug[2] = monPower;
+ }
+#endif
}
+#ifdef VTX_COMMON
+// Interface to common VTX API
+
+vtxDevType_e vtxSAGetDeviceType(void)
+{
+ return VTXDEV_SMARTAUDIO;
+}
+
+bool vtxSAIsReady(void)
+{
+ return !(saDevice.version == 0);
+}
+
+void vtxSASetBandChan(uint8_t band, uint8_t chan)
+{
+ if (band && chan)
+ saSetBandChan(band - 1, chan - 1);
+}
+
+void vtxSASetPowerByIndex(uint8_t index)
+{
+ if (index == 0) {
+ // SmartAudio doesn't support power off.
+ return;
+ }
+
+ saSetPowerByIndex(index - 1);
+}
+
+void vtxSASetPitmode(uint8_t onoff)
+{
+ if (!(vtxSAIsReady() && (saDevice.version == 2)))
+ return;
+
+ if (onoff) {
+ // SmartAudio can not turn pit mode on by software.
+ return;
+ }
+
+ uint8_t newmode = SA_MODE_CLR_PITMODE;
+
+ if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE)
+ newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+
+ if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+ newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+
+ saSetMode(newmode);
+
+ return true;
+}
+
+bool vtxSAGetBandChan(uint8_t *pBand, uint8_t *pChan)
+{
+ if (!vtxSAIsReady())
+ return false;
+
+ *pBand = (saDevice.chan / 8) + 1;
+ *pChan = (saDevice.chan % 8) + 1;
+ return true;
+}
+
+bool vtxSAGetPowerIndex(uint8_t *pIndex)
+{
+ if (!vtxSAIsReady())
+ return false;
+
+ *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
+ return true;
+}
+
+bool vtxSAGetPitmode(uint8_t *pOnoff)
+{
+ if (!(vtxSAIsReady() && (saDevice.version == 2)))
+ return false;
+
+ *pOnoff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
+ return true;
+}
+
+static vtxVTable_t saVTable = {
+ .process = vtxSAProcess,
+ .getDeviceType = vtxSAGetDeviceType,
+ .isReady = vtxSAIsReady,
+ .setBandChan = vtxSASetBandChan,
+ .setPowerByIndex = vtxSASetPowerByIndex,
+ .setPitmode = vtxSASetPitmode,
+ .getBandChan = vtxSAGetBandChan,
+ .getPowerIndex = vtxSAGetPowerIndex,
+ .getPitmode = vtxSAGetPitmode,
+};
+#endif // VTX_COMMON
+
#ifdef CMS
// Interface to CMS
@@ -806,7 +945,7 @@ if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
else
saCmsPitFMode = 0;
- saCmsStatusString[0] = "-FP"[(saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE];
+ saCmsStatusString[0] = "-FR"[saCmsOpmodel];
saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8];
saCmsStatusString[3] = '1' + (saDevice.chan % 8);
@@ -846,15 +985,13 @@ static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
return 0;
}
-dprintf(("saCmsConfigBand: band req %d ", saCmsBand));
-
if (saCmsBand == 0) {
// Bouce back, no going back to undef state
saCmsBand = 1;
return 0;
}
- if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
+ if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
saSetBandChan(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
@@ -879,7 +1016,7 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
return 0;
}
- if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
+ if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
saSetBandChan(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
@@ -904,7 +1041,8 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
return 0;
}
- saSetPowerByIndex(saCmsPower - 1);
+ if (saCmsOpmodel == SACMS_OPMODEL_FREE)
+ saSetPowerByIndex(saCmsPower - 1);
return 0;
}
@@ -985,7 +1123,7 @@ static CMS_Menu saCmsMenuStats = {
static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL };
-static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChannelNames[0], NULL };
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames, NULL };
static const char * const saCmsPowerNames[] = {
"---",
@@ -1048,11 +1186,21 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
UNUSED(self);
if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
+ // Race model
+ // Setup band, freq and power.
+
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+ saSetPowerByIndex(saCmsPower - 1);
+
+ // If in pit mode, cancel it.
+
if (saCmsPitFMode == 0)
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
else
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
} else {
+ // Freestyle model
+ // Setup band and freq / user freq
if (saCmsFselMode == 0)
saSetBandChan(saCmsBand - 1, saCmsChan - 1);
else
diff --git a/src/main/io/vtx_smartaudio.c.orig b/src/main/io/vtx_smartaudio.c.orig
new file mode 100644
index 0000000000..575c7dedec
--- /dev/null
+++ b/src/main/io/vtx_smartaudio.c.orig
@@ -0,0 +1,1422 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/* Created by jflyper */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+#include "string.h"
+#include "common/printf.h"
+#include "common/utils.h"
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "drivers/vtx_var.h"
+#include "drivers/vtx_common.h"
+#include "io/serial.h"
+#include "io/vtx_smartaudio.h"
+#include "io/vtx_common.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/pid.h"
+#include "config/config_master.h"
+
+#include "build/build_config.h"
+
+//#define SMARTAUDIO_DPRINTF
+//#define SMARTAUDIO_DEBUG_MONITOR
+
+#ifdef SMARTAUDIO_DPRINTF
+
+#ifdef OMNIBUSF4
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
+#else
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1
+#endif
+
+serialPort_t *debugSerialPort = NULL;
+#define dprintf(x) if (debugSerialPort) printf x
+#else
+#define dprintf(x)
+#endif
+
+#include "build/debug.h"
+
+#ifdef CMS
+static void saUpdateStatusString(void); // Forward
+#endif
+
+static serialPort_t *smartAudioSerialPort = NULL;
+
+#if defined(CMS) || defined(VTX_COMMON)
+static const char * const saPowerNames[] = {
+ "---", "25 ", "200", "500", "800",
+};
+#endif
+
+#ifdef VTX_COMMON
+static vtxVTable_t saVTable; // Forward
+static vtxDevice_t vtxSmartAudio = {
+ .vTable = &saVTable,
+ .numBand = 5,
+ .numChan = 8,
+ .numPower = 4,
+ .bandNames = (char **)vtx58BandNames,
+ .chanNames = (char **)vtx58ChanNames,
+ .powerNames = (char **)saPowerNames,
+};
+#endif
+
+// SmartAudio command and response codes
+enum {
+ SA_CMD_NONE = 0x00,
+ SA_CMD_GET_SETTINGS = 0x01,
+ SA_CMD_SET_POWER,
+ SA_CMD_SET_CHAN,
+ SA_CMD_SET_FREQ,
+ SA_CMD_SET_MODE,
+ SA_CMD_GET_SETTINGS_V2 = 0x09 // Response only
+} smartAudioCommand_e;
+
+// This is not a good design; can't distinguish command from response this way.
+#define SACMD(cmd) (((cmd) << 1) | 1)
+
+// opmode flags, GET side
+#define SA_MODE_GET_FREQ_BY_FREQ 1
+#define SA_MODE_GET_PITMODE 2
+#define SA_MODE_GET_IN_RANGE_PITMODE 4
+#define SA_MODE_GET_OUT_RANGE_PITMODE 8
+#define SA_MODE_GET_UNLOCK 16
+#define SA_MODE_GET_DEFERRED_FREQ 32
+
+#define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE)
+#define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE))
+#define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))
+
+// opmode flags, SET side
+#define SA_MODE_SET_IN_RANGE_PITMODE 1 // Immediate
+#define SA_MODE_SET_OUT_RANGE_PITMODE 2 // Immediate
+#define SA_MODE_CLR_PITMODE 4 // Immediate
+#define SA_MODE_SET_UNLOCK 8
+#define SA_MODE_SET_LOCK 0 // ~UNLOCK
+#define SA_MODE_SET_DEFERRED_FREQ 16
+
+// SetFrequency flags, for pit mode frequency manipulation
+#define SA_FREQ_GETPIT (1 << 14)
+#define SA_FREQ_SETPIT (1 << 15)
+#define SA_FREQ_MASK (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
+
+// Statistical counters, for user side trouble shooting.
+
+typedef struct smartAudioStat_s {
+ uint16_t pktsent;
+ uint16_t pktrcvd;
+ uint16_t badpre;
+ uint16_t badlen;
+ uint16_t crc;
+ uint16_t ooopresp;
+ uint16_t badcode;
+} smartAudioStat_t;
+
+static smartAudioStat_t saStat = {
+ .pktsent = 0,
+ .pktrcvd = 0,
+ .badpre = 0,
+ .badlen = 0,
+ .crc = 0,
+ .ooopresp = 0,
+ .badcode = 0,
+};
+
+typedef struct saPowerTable_s {
+ int rfpower;
+ int16_t valueV1;
+ int16_t valueV2;
+} saPowerTable_t;
+
+static saPowerTable_t saPowerTable[] = {
+ { 25, 7, 0 },
+ { 200, 16, 1 },
+ { 500, 25, 2 },
+ { 800, 40, 3 },
+};
+
+// Last received device ('hard') states
+
+typedef struct smartAudioDevice_s {
+ int8_t version;
+ int8_t chan;
+ int8_t power;
+ int8_t mode;
+ uint16_t freq;
+ uint16_t orfreq;
+} smartAudioDevice_t;
+
+static smartAudioDevice_t saDevice = {
+ .version = 0,
+ .chan = -1,
+ .power = -1,
+ .mode = 0,
+ .freq = 0,
+ .orfreq = 0,
+};
+
+static smartAudioDevice_t saDevicePrev = {
+ .version = 0,
+};
+
+// XXX Possible compliance problem here. Need LOCK/UNLOCK menu?
+static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
+
+// XXX Should be configurable by user?
+static bool saDeferred = true; // saCms variable?
+
+// Receive frame reassembly buffer
+#define SA_MAX_RCVLEN 11
+static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
+
+//
+// CRC8 computations
+//
+
+#define POLYGEN 0xd5
+
+static uint8_t CRC8(const uint8_t *data, const int8_t len)
+{
+ uint8_t crc = 0; /* start with 0 so first byte can be 'xored' in */
+ uint8_t currByte;
+
+ for (int i = 0 ; i < len ; i++) {
+ currByte = data[i];
+
+ crc ^= currByte; /* XOR-in the next input byte */
+
+ for (int i = 0; i < 8; i++) {
+ if ((crc & 0x80) != 0) {
+ crc = (uint8_t)((crc << 1) ^ POLYGEN);
+ } else {
+ crc <<= 1;
+ }
+ }
+ }
+ return crc;
+}
+
+
+static void saPrintSettings(void)
+{
+#ifdef SMARTAUDIO_DPRINTF
+ dprintf(("Current status: version: %d\r\n", saDevice.version));
+ dprintf((" mode(0x%x): fmode=%s", saDevice.mode, (saDevice.mode & 1) ? "freq" : "chan"));
+ dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
+ dprintf((" inb=%s", (saDevice.mode & 4) ? "on " : "off"));
+ dprintf((" outb=%s", (saDevice.mode & 8) ? "on " : "off"));
+ dprintf((" lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked"));
+ dprintf((" deferred=%s\r\n", (saDevice.mode & 32) ? "on" : "off"));
+ dprintf((" chan: %d ", saDevice.chan));
+ dprintf(("freq: %d ", saDevice.freq));
+ dprintf(("power: %d ", saDevice.power));
+ dprintf(("pitfreq: %d ", saDevice.orfreq));
+ dprintf(("\r\n"));
+#endif
+}
+
+static int saDacToPowerIndex(int dac)
+{
+ int idx;
+
+ for (idx = 0 ; idx < 4 ; idx++) {
+ if (saPowerTable[idx].valueV1 <= dac)
+ return(idx);
+ }
+ return(3);
+}
+
+//
+// Autobauding
+//
+
+#define SMARTBAUD_MIN 4800
+#define SMARTBAUD_MAX 4950
+uint16_t sa_smartbaud = SMARTBAUD_MIN;
+static int sa_adjdir = 1; // -1=going down, 1=going up
+static int sa_baudstep = 50;
+
+#define SMARTAUDIO_CMD_TIMEOUT 120
+
+static void saAutobaud(void)
+{
+ if (saStat.pktsent < 10)
+ // Not enough samples collected
+ return;
+
+#if 0
+ dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
+ sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent)));
+#endif
+
+ if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) {
+ // This is okay
+ saStat.pktsent = 0; // Should be more moderate?
+ saStat.pktrcvd = 0;
+ return;
+ }
+
+ dprintf(("autobaud: adjusting\r\n"));
+
+ if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) {
+ sa_adjdir = -1;
+ dprintf(("autobaud: now going down\r\n"));
+ } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) {
+ sa_adjdir = 1;
+ dprintf(("autobaud: now going up\r\n"));
+ }
+
+ sa_smartbaud += sa_baudstep * sa_adjdir;
+
+ dprintf(("autobaud: %d\r\n", sa_smartbaud));
+
+ smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud);
+
+ saStat.pktsent = 0;
+ saStat.pktrcvd = 0;
+}
+
+// Transport level variables
+
+static uint32_t sa_lastTransmission = 0;
+static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
+static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
+static int sa_oslen; // And associate length
+
+#ifdef CMS
+void saCmsUpdate(void);
+#endif
+
+static void saProcessResponse(uint8_t *buf, int len)
+{
+ uint8_t resp = buf[0];
+
+ if (resp == sa_outstanding) {
+ sa_outstanding = SA_CMD_NONE;
+ } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) {
+ sa_outstanding = SA_CMD_NONE;
+ } else {
+ saStat.ooopresp++;
+ dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp));
+ }
+
+ switch(resp) {
+ case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
+ case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
+ if (len < 7)
+ break;
+
+ saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
+ saDevice.chan = buf[2];
+ saDevice.power = buf[3];
+ saDevice.mode = buf[4];
+ saDevice.freq = (buf[5] << 8)|buf[6];
+
+#ifdef SMARTAUDIO_DEBUG_MONITOR
+ debug[0] = saDevice.version * 100 + saDevice.mode;
+ debug[1] = saDevice.chan;
+ debug[2] = saDevice.freq;
+ debug[3] = saDevice.power;
+#endif
+ break;
+
+ case SA_CMD_SET_POWER: // Set Power
+ break;
+
+ case SA_CMD_SET_CHAN: // Set Channel
+ break;
+
+ case SA_CMD_SET_FREQ: // Set Frequency
+ if (len < 5)
+ break;
+
+ uint16_t freq = (buf[2] << 8)|buf[3];
+
+ if (freq & SA_FREQ_GETPIT) {
+ saDevice.orfreq = freq & SA_FREQ_MASK;
+ dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq));
+ } else if (freq & SA_FREQ_SETPIT) {
+ saDevice.orfreq = freq & SA_FREQ_MASK;
+ dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq));
+ } else {
+ saDevice.freq = freq;
+ dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq));
+ }
+ break;
+
+ case SA_CMD_SET_MODE: // Set Mode
+ dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2]));
+ break;
+
+ default:
+ saStat.badcode++;
+ return;
+ }
+
+ // Debug
+ if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t)))
+ saPrintSettings();
+ saDevicePrev = saDevice;
+
+#ifdef VTX_COMMON
+ // Todo: Update states in saVtxDevice?
+#endif
+
+#ifdef CMS
+ // Export current device status for CMS
+ saCmsUpdate();
+ saUpdateStatusString();
+#endif
+}
+
+//
+// Datalink
+//
+
+static void saReceiveFramer(uint8_t c)
+{
+
+ static enum saFramerState_e {
+ S_WAITPRE1, // Waiting for preamble 1 (0xAA)
+ S_WAITPRE2, // Waiting for preamble 2 (0x55)
+ S_WAITRESP, // Waiting for response code
+ S_WAITLEN, // Waiting for length
+ S_DATA, // Receiving data
+ S_WAITCRC, // Waiting for CRC
+ } state = S_WAITPRE1;
+
+ static int len;
+ static int dlen;
+
+ switch(state) {
+ case S_WAITPRE1:
+ if (c == 0xAA) {
+ state = S_WAITPRE2;
+ } else {
+ state = S_WAITPRE1; // Don't need this (no change)
+ }
+ break;
+
+ case S_WAITPRE2:
+ if (c == 0x55) {
+ state = S_WAITRESP;
+ } else {
+ saStat.badpre++;
+ state = S_WAITPRE1;
+ }
+ break;
+
+ case S_WAITRESP:
+ sa_rbuf[0] = c;
+ state = S_WAITLEN;
+ break;
+
+ case S_WAITLEN:
+ sa_rbuf[1] = c;
+ len = c;
+
+ if (len > SA_MAX_RCVLEN - 2) {
+ saStat.badlen++;
+ state = S_WAITPRE1;
+ } else if (len == 0) {
+ state = S_WAITCRC;
+ } else {
+ dlen = 0;
+ state = S_DATA;
+ }
+ break;
+
+ case S_DATA:
+ // XXX Should check buffer overflow (-> saerr_overflow)
+ sa_rbuf[2 + dlen] = c;
+ if (++dlen == len) {
+ state = S_WAITCRC;
+ }
+ break;
+
+ case S_WAITCRC:
+ if (CRC8(sa_rbuf, 2 + len) == c) {
+ // Got a response
+ saProcessResponse(sa_rbuf, len + 2);
+ saStat.pktrcvd++;
+ } else if (sa_rbuf[0] & 1) {
+ // Command echo
+ // XXX There is an exceptional case (V2 response)
+ // XXX Should check crc in the command format?
+ } else {
+ saStat.crc++;
+ }
+ state = S_WAITPRE1;
+ break;
+ }
+}
+
+static void saSendFrame(uint8_t *buf, int len)
+{
+ int i;
+
+ serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
+
+ for (i = 0 ; i < len ; i++)
+ serialWrite(smartAudioSerialPort, buf[i]);
+
+ serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
+
+ sa_lastTransmission = millis();
+ saStat.pktsent++;
+}
+
+/*
+ * Retransmission and command queuing
+ *
+ * The transport level support includes retransmission on response timeout
+ * and command queueing.
+ *
+ * Resend buffer:
+ * The smartaudio returns response for valid command frames in no less
+ * than 60msec, which we can't wait. So there's a need for a resend buffer.
+ *
+ * Command queueing:
+ * The driver autonomously sends GetSettings command for auto-bauding,
+ * asynchronous to user initiated commands; commands issued while another
+ * command is outstanding must be queued for later processing.
+ * The queueing also handles the case in which multiple commands are
+ * required to implement a user level command.
+ */
+
+// Retransmission
+
+static void saResendCmd(void)
+{
+ saSendFrame(sa_osbuf, sa_oslen);
+}
+
+static void saSendCmd(uint8_t *buf, int len)
+{
+ int i;
+
+ for (i = 0 ; i < len ; i++)
+ sa_osbuf[i] = buf[i];
+
+ sa_oslen = len;
+ sa_outstanding = (buf[2] >> 1);
+
+ saSendFrame(sa_osbuf, sa_oslen);
+}
+
+// Command queue management
+
+typedef struct saCmdQueue_s {
+ uint8_t *buf;
+ int len;
+} saCmdQueue_t;
+
+#define SA_QSIZE 4 // 1 heartbeat (GetSettings) + 2 commands + 1 slack
+static saCmdQueue_t sa_queue[SA_QSIZE];
+static uint8_t sa_qhead = 0;
+static uint8_t sa_qtail = 0;
+
+#ifdef DPRINTF_SMARTAUDIO
+static int saQueueLength()
+{
+ if (sa_qhead >= sa_qtail) {
+ return sa_qhead - sa_qtail;
+ } else {
+ return SA_QSIZE + sa_qhead - sa_qtail;
+ }
+}
+#endif
+
+static bool saQueueEmpty()
+{
+ return sa_qhead == sa_qtail;
+}
+
+static bool saQueueFull()
+{
+ return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail;
+}
+
+static void saQueueCmd(uint8_t *buf, int len)
+{
+ if (saQueueFull())
+ return;
+
+ sa_queue[sa_qhead].buf = buf;
+ sa_queue[sa_qhead].len = len;
+ sa_qhead = (sa_qhead + 1) % SA_QSIZE;
+}
+
+static void saSendQueue(void)
+{
+ if (saQueueEmpty())
+ return;
+
+ saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
+ sa_qtail = (sa_qtail + 1) % SA_QSIZE;
+}
+
+// Individual commands
+
+static void saGetSettings(void)
+{
+ static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
+
+ saQueueCmd(bufGetSettings, 5);
+}
+
+static void saSetFreq(uint16_t freq)
+{
+ static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
+
+ if (freq & SA_FREQ_GETPIT) {
+ dprintf(("smartAudioSetFreq: GETPIT\r\n"));
+ } else if (freq & SA_FREQ_SETPIT) {
+ dprintf(("smartAudioSetFreq: SETPIT %d\r\n", freq & SA_FREQ_MASK));
+ } else {
+ dprintf(("smartAudioSetFreq: SET %d\r\n", freq));
+ }
+
+ buf[4] = (freq >> 8) & 0xff;
+ buf[5] = freq & 0xff;
+ buf[6] = CRC8(buf, 6);
+
+ saQueueCmd(buf, 7);
+}
+
+#if 0
+static void saSetPitFreq(uint16_t freq)
+{
+ saSetFreq(freq | SA_FREQ_SETPIT);
+}
+
+static void saGetPitFreq(void)
+{
+ saSetFreq(SA_FREQ_GETPIT);
+}
+#endif
+
+void saSetBandChan(uint8_t band, uint8_t chan)
+{
+ static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
+
+ buf[4] = band * 8 + chan;
+ buf[5] = CRC8(buf, 5);
+
+ saQueueCmd(buf, 6);
+}
+
+static void saSetMode(int mode)
+{
+ static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
+
+ buf[4] = (mode & 0x3f)|saLockMode;
+ buf[5] = CRC8(buf, 5);
+
+ saQueueCmd(buf, 6);
+}
+
+void saSetPowerByIndex(uint8_t index)
+{
+ static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+
+ dprintf(("saSetPowerByIndex: index %d\r\n", index));
+
+ if (saDevice.version == 0) {
+ // Unknown or yet unknown version.
+ return;
+ }
+
+ if (index > 3)
+ return;
+
+ buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
+ buf[5] = CRC8(buf, 5);
+ saQueueCmd(buf, 6);
+}
+
+bool smartAudioInit()
+{
+#ifdef SMARTAUDIO_DPRINTF
+ // Setup debugSerialPort
+
+ debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
+ if (debugSerialPort) {
+ setPrintfSerialPort(debugSerialPort);
+ dprintf(("smartAudioInit: OK\r\n"));
+ }
+#endif
+
+ serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
+ if (portConfig) {
+ smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
+ }
+
+ if (!smartAudioSerialPort) {
+ return false;
+ }
+
+ vtxSmartAudio.vTable = &saVTable;
+ vtxCommonRegisterDevice(&vtxSmartAudio);
+
+ return true;
+}
+
+void vtxSAProcess(uint32_t now)
+{
+ static bool initialSent = false;
+
+ if (smartAudioSerialPort == NULL)
+ return;
+
+ while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
+ uint8_t c = serialRead(smartAudioSerialPort);
+ saReceiveFramer((uint16_t)c);
+ }
+
+ // Re-evaluate baudrate after each frame reception
+ saAutobaud();
+
+ if (!initialSent) {
+ saGetSettings();
+ saSetFreq(SA_FREQ_GETPIT);
+ saSendQueue();
+ initialSent = true;
+ return;
+ }
+
+ if ((sa_outstanding != SA_CMD_NONE)
+ && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
+ // Last command timed out
+ // dprintf(("process: resending 0x%x\r\n", sa_outstanding));
+ // XXX Todo: Resend termination and possible offline transition
+ saResendCmd();
+ } else if (!saQueueEmpty()) {
+ // Command pending. Send it.
+ // dprintf(("process: sending queue\r\n"));
+ saSendQueue();
+ } else if (now - sa_lastTransmission >= 1000) {
+ // Heart beat for autobauding
+ //dprintf(("process: sending heartbeat\r\n"));
+ saGetSettings();
+ saSendQueue();
+ }
+
+#ifdef SMARTAUDIO_TEST_VTX_COMMON
+ // Testing VTX_COMMON API
+ {
+ static uint32_t lastMonitorUs = 0;
+ if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000)
+ return;
+
+ static uint8_t monBand;
+ static uint8_t monChan;
+ static uint8_t monPower;
+
+ vtxCommonGetBandChan(&monBand, &monChan);
+ vtxCommonGetPowerIndex(&monPower);
+ debug[0] = monBand;
+ debug[1] = monChan;
+ debug[2] = monPower;
+ }
+#endif
+}
+
+#ifdef VTX_COMMON
+// Interface to common VTX API
+
+vtxDevType_e vtxSAGetDeviceType(void)
+{
+ return VTXDEV_SMARTAUDIO;
+}
+
+bool vtxSAIsReady(void)
+{
+ return !(saDevice.version == 0);
+}
+
+void vtxSASetBandChan(uint8_t band, uint8_t chan)
+{
+ if (band && chan)
+ saSetBandChan(band - 1, chan - 1);
+}
+
+void vtxSASetPowerByIndex(uint8_t index)
+{
+ if (index == 0) {
+ // SmartAudio doesn't support power off.
+ return;
+ }
+
+ saSetPowerByIndex(index - 1);
+}
+
+void vtxSASetPitmode(uint8_t onoff)
+{
+ if (!(vtxSAIsReady() && (saDevice.version == 2)))
+ return;
+
+ if (onoff) {
+ // SmartAudio can not turn pit mode on by software.
+ return;
+ }
+
+ uint8_t newmode = SA_MODE_CLR_PITMODE;
+
+ if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE)
+ newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+
+ if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+ newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+
+ saSetMode(newmode);
+
+ return true;
+}
+
+bool vtxSAGetBandChan(uint8_t *pBand, uint8_t *pChan)
+{
+ if (!vtxSAIsReady())
+ return false;
+
+ *pBand = (saDevice.chan / 8) + 1;
+ *pChan = (saDevice.chan % 8) + 1;
+ return true;
+}
+
+bool vtxSAGetPowerIndex(uint8_t *pIndex)
+{
+ if (!vtxSAIsReady())
+ return false;
+
+ *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
+ return true;
+}
+
+bool vtxSAGetPitmode(uint8_t *pOnoff)
+{
+ if (!(vtxSAIsReady() && (saDevice.version == 2)))
+ return false;
+
+ *pOnoff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
+ return true;
+}
+
+static vtxVTable_t saVTable = {
+ .process = vtxSAProcess,
+ .getDeviceType = vtxSAGetDeviceType,
+ .isReady = vtxSAIsReady,
+ .setBandChan = vtxSASetBandChan,
+ .setPowerByIndex = vtxSASetPowerByIndex,
+ .setPitmode = vtxSASetPitmode,
+ .getBandChan = vtxSAGetBandChan,
+ .getPowerIndex = vtxSAGetPowerIndex,
+ .getPitmode = vtxSAGetPitmode,
+};
+#endif // VTX_COMMON
+
+#ifdef CMS
+
+// Interface to CMS
+
+// Operational Model and RF modes (CMS)
+
+#define SACMS_OPMODEL_UNDEF 0 // Not known yet
+#define SACMS_OPMODEL_FREE 1 // Freestyle model: Power up transmitting
+#define SACMS_OPMODEL_RACE 2 // Race model: Power up in pit mode
+
+uint8_t saCmsOpmodel = SACMS_OPMODEL_UNDEF;
+
+#define SACMS_TXMODE_NODEF 0
+#define SACMS_TXMODE_PIT_OUTRANGE 1
+#define SACMS_TXMODE_PIT_INRANGE 2
+#define SACMS_TXMODE_ACTIVE 3
+
+uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently used
+
+uint8_t saCmsBand = 0;
+uint8_t saCmsChan = 0;
+uint8_t saCmsPower = 0;
+
+// Frequency derived from channel table (used for reference in band/chan mode)
+uint16_t saCmsFreqRef = 0;
+
+uint16_t saCmsDeviceFreq = 0;
+
+uint8_t saCmsDeviceStatus = 0;
+uint8_t saCmsPower;
+uint8_t saCmsPitFMode; // In-Range or Out-Range
+uint8_t saCmsFselMode; // Channel(0) or User defined(1)
+
+uint16_t saCmsORFreq = 0; // POR frequency
+uint16_t saCmsORFreqNew; // POR frequency
+
+uint16_t saCmsUserFreq = 0; // User defined frequency
+uint16_t saCmsUserFreqNew; // User defined frequency
+
+void saCmsUpdate(void)
+{
+// XXX Take care of pit mode update somewhere???
+
+ if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
+ // This is a first valid response to GET_SETTINGS.
+ saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
+
+ saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
+
+ saCmsBand = (saDevice.chan / 8) + 1;
+ saCmsChan = (saDevice.chan % 8) + 1;
+ saCmsFreqRef = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8];
+
+ saCmsDeviceFreq = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8];
+
+ if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
+ saCmsRFState = SACMS_TXMODE_ACTIVE;
+ } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
+ saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
+ } else {
+ saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
+ }
+
+ if (saDevice.version == 2) {
+ saCmsPower = saDevice.power + 1; // XXX Take care V1
+ } else {
+ saCmsPower = saDacToPowerIndex(saDevice.power) + 1;
+ }
+ }
+
+ saUpdateStatusString();
+}
+
+char saCmsStatusString[31] = "- -- ---- ---";
+// m bc ffff ppp
+// 0123456789012
+
+static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self);
+static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self);
+static long saCmsConfigBandByGvar(displayPort_t *, const void *self);
+static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
+static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);
+
+static void saUpdateStatusString(void)
+{
+ if (saDevice.version == 0)
+ return;
+
+// XXX These should be done somewhere else
+if (saCmsDeviceStatus == 0 && saDevice.version != 0)
+ saCmsDeviceStatus = saDevice.version;
+if (saCmsORFreq == 0 && saDevice.orfreq != 0)
+ saCmsORFreq = saDevice.orfreq;
+if (saCmsUserFreq == 0 && saDevice.freq != 0)
+ saCmsUserFreq = saDevice.freq;
+
+if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+ saCmsPitFMode = 1;
+else
+ saCmsPitFMode = 0;
+
+ saCmsStatusString[0] = "-FR"[saCmsOpmodel];
+ saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8];
+ saCmsStatusString[3] = '1' + (saDevice.chan % 8);
+
+ if ((saDevice.mode & SA_MODE_GET_PITMODE)
+ && (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))
+ tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.orfreq);
+ else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ)
+ tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq);
+ else
+ tfp_sprintf(&saCmsStatusString[5], "%4d",
+ vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]);
+
+ saCmsStatusString[9] = ' ';
+
+ if (saDevice.mode & SA_MODE_GET_PITMODE) {
+ saCmsStatusString[10] = 'P';
+ if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
+ saCmsStatusString[11] = 'I';
+ } else {
+ saCmsStatusString[11] = 'O';
+ }
+ saCmsStatusString[12] = 'R';
+ saCmsStatusString[13] = 0;
+ } else {
+ tfp_sprintf(&saCmsStatusString[10], "%3d", (saDevice.version == 2) ? saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower);
+ }
+}
+
+static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (saDevice.version == 0) {
+ // Bounce back; not online yet
+ saCmsBand = 0;
+ return 0;
+ }
+
+ if (saCmsBand == 0) {
+ // Bouce back, no going back to undef state
+ saCmsBand = 1;
+ return 0;
+ }
+
+ if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+
+ saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
+
+ return 0;
+}
+
+static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (saDevice.version == 0) {
+ // Bounce back; not online yet
+ saCmsChan = 0;
+ return 0;
+ }
+
+ if (saCmsChan == 0) {
+ // Bounce back; no going back to undef state
+ saCmsChan = 1;
+ return 0;
+ }
+
+ if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+
+ saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
+
+ return 0;
+}
+
+static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (saDevice.version == 0) {
+ // Bounce back; not online yet
+ saCmsPower = 0;
+ return 0;
+ }
+
+ if (saCmsPower == 0) {
+ // Bouce back; no going back to undef state
+ saCmsPower = 1;
+ return 0;
+ }
+
+ if (saCmsOpmodel == SACMS_OPMODEL_FREE)
+ saSetPowerByIndex(saCmsPower - 1);
+
+ return 0;
+}
+
+static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode));
+
+ if (saCmsPitFMode == 0) {
+ saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
+ } else {
+ saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
+ }
+
+ return 0;
+}
+
+static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ uint8_t opmodel = saCmsOpmodel;
+
+ dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel));
+
+ if (opmodel == SACMS_OPMODEL_FREE) {
+ // VTX should power up transmitting.
+ // Turn off In-Range and Out-Range bits
+ saSetMode(0);
+ } else if (opmodel == SACMS_OPMODEL_RACE) {
+ // VTX should power up in pit mode.
+ // Default PitFMode is in-range to prevent users without
+ // out-range receivers from getting blinded.
+ saCmsPitFMode = 0;
+ saCmsConfigPitFModeByGvar(pDisp, self);
+ } else {
+ // Trying to go back to unknown state; bounce back
+ saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1;
+ }
+
+ return 0;
+}
+
+static const char * const saCmsDeviceStatusNames[] = {
+ "OFFL",
+ "ONL V1",
+ "ONL V2",
+};
+
+static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames };
+
+static OSD_Entry saCmsMenuStatsEntries[] = {
+ { "- SA STATS -", OME_Label, NULL, NULL, 0 },
+ { "STATUS", OME_TAB, NULL, &saCmsEntOnline, DYNAMIC },
+ { "BAUDRATE", OME_UINT16, NULL, &(OSD_UINT16_t){ &sa_smartbaud, 0, 0, 0 }, DYNAMIC },
+ { "SENT", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktsent, 0, 0, 0 }, DYNAMIC },
+ { "RCVD", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktrcvd, 0, 0, 0 }, DYNAMIC },
+ { "BADPRE", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badpre, 0, 0, 0 }, DYNAMIC },
+ { "BADLEN", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badlen, 0, 0, 0 }, DYNAMIC },
+ { "CRCERR", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.crc, 0, 0, 0 }, DYNAMIC },
+ { "OOOERR", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.ooopresp, 0, 0, 0 }, DYNAMIC },
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuStats = {
+ .GUARD_text = "XSAST",
+ .GUARD_type = OME_MENU,
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuStatsEntries
+};
+
+static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL };
+
+<<<<<<< HEAD
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChanNames, NULL };
+=======
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChannelNames[0], NULL };
+
+static const char * const saCmsPowerNames[] = {
+ "---",
+ "25 ",
+ "200",
+ "500",
+ "800",
+};
+>>>>>>> betaflight/master
+
+static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saPowerNames};
+
+static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 };
+
+static const char * const saCmsOpmodelNames[] = {
+ "----",
+ "FREE",
+ "RACE",
+};
+
+static const char * const saCmsFselModeNames[] = {
+ "CHAN",
+ "USER"
+};
+
+static const char * const saCmsPitFModeNames[] = {
+ "PIR",
+ "POR"
+};
+
+static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
+
+static long sacms_SetupTopMenu(void); // Forward
+
+static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (saCmsFselMode == 0) {
+ // CHAN
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+ } else {
+ // USER: User frequency mode is only available in FREE opmodel.
+ if (saCmsOpmodel == SACMS_OPMODEL_FREE) {
+ saSetFreq(saCmsUserFreq);
+ } else {
+ // Bounce back
+ saCmsFselMode = 0;
+ }
+ }
+
+ sacms_SetupTopMenu();
+
+ return 0;
+}
+
+static long saCmsCommence(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
+ // Race model
+ // Setup band, freq and power.
+
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+ saSetPowerByIndex(saCmsPower - 1);
+
+ // If in pit mode, cancel it.
+
+ if (saCmsPitFMode == 0)
+ saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
+ else
+ saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
+ } else {
+ // Freestyle model
+ // Setup band and freq / user freq
+ if (saCmsFselMode == 0)
+ saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+ else
+ saSetFreq(saCmsUserFreq);
+ }
+
+ return MENU_CHAIN_BACK;
+}
+
+static long saCmsSetPORFreqOnEnter(void)
+{
+ saCmsORFreqNew = saCmsORFreq;
+
+ return 0;
+}
+
+static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT);
+
+ return 0;
+}
+
+static char *saCmsORFreqGetString(void)
+{
+ static char pbuf[5];
+
+ tfp_sprintf(pbuf, "%4d", saCmsORFreq);
+
+ return pbuf;
+}
+
+static char *saCmsUserFreqGetString(void)
+{
+ static char pbuf[5];
+
+ tfp_sprintf(pbuf, "%4d", saCmsUserFreq);
+
+ return pbuf;
+}
+
+static long saCmsSetUserFreqOnEnter(void)
+{
+ saCmsUserFreqNew = saCmsUserFreq;
+
+ return 0;
+}
+
+static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ saCmsUserFreq = saCmsUserFreqNew;
+ saSetFreq(saCmsUserFreq);
+
+ return 0;
+}
+
+static OSD_Entry saCmsMenuPORFreqEntries[] = {
+ { "- POR FREQ -", OME_Label, NULL, NULL, 0 },
+
+ { "CUR FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsORFreq, 5000, 5900, 0 }, DYNAMIC },
+ { "NEW FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 }, 0 },
+ { "SET", OME_Funcall, saCmsSetPORFreq, NULL, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuPORFreq =
+{
+ .GUARD_text = "XSAPOR",
+ .GUARD_type = OME_MENU,
+ .onEnter = saCmsSetPORFreqOnEnter,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuPORFreqEntries,
+};
+
+static OSD_Entry saCmsMenuUserFreqEntries[] = {
+ { "- USER FREQ -", OME_Label, NULL, NULL, 0 },
+
+ { "CUR FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsUserFreq, 5000, 5900, 0 }, DYNAMIC },
+ { "NEW FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 }, 0 },
+ { "SET", OME_Funcall, saCmsSetUserFreq, NULL, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuUserFreq =
+{
+ .GUARD_text = "XSAUFQ",
+ .GUARD_type = OME_MENU,
+ .onEnter = saCmsSetUserFreqOnEnter,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuUserFreqEntries,
+};
+
+static OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames };
+
+static OSD_Entry saCmsMenuConfigEntries[] = {
+ { "- SA CONFIG -", OME_Label, NULL, NULL, 0 },
+
+ { "OP MODEL", OME_TAB, saCmsConfigOpmodelByGvar, &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, 0 },
+ { "FSEL MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFselMode, 0 },
+ { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 },
+ { "POR FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq, OPTSTRING },
+ { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuConfig = {
+ .GUARD_text = "XSACFG",
+ .GUARD_type = OME_MENU,
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuConfigEntries
+};
+
+static OSD_Entry saCmsMenuCommenceEntries[] = {
+ { "CONFIRM", OME_Label, NULL, NULL, 0 },
+
+ { "YES", OME_Funcall, saCmsCommence, NULL, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuCommence = {
+ .GUARD_text = "XVTXCOM",
+ .GUARD_type = OME_MENU,
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuCommenceEntries,
+};
+
+static OSD_Entry saCmsMenuFreqModeEntries[] = {
+ { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+ { "", OME_Label, NULL, saCmsStatusString, DYNAMIC },
+ { "FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString, &saCmsMenuUserFreq, OPTSTRING },
+ { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 },
+ { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 },
+ { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static OSD_Entry saCmsMenuChanModeEntries[] =
+{
+ { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+ { "", OME_Label, NULL, saCmsStatusString, DYNAMIC },
+ { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 },
+ { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 },
+ { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreqRef, DYNAMIC },
+ { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 },
+ { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 },
+ { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static OSD_Entry saCmsMenuOfflineEntries[] =
+{
+ { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+ { "", OME_Label, NULL, saCmsStatusString, DYNAMIC },
+ { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0 },
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+
+CMS_Menu cmsx_menuVtxSmartAudio; // Forward
+
+static long sacms_SetupTopMenu(void)
+{
+ if (saCmsDeviceStatus) {
+ if (saCmsFselMode == 0)
+ cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
+ else
+ cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
+ } else {
+ cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries;
+ }
+
+ return 0;
+}
+
+CMS_Menu cmsx_menuVtxSmartAudio = {
+ .GUARD_text = "XVTXSA",
+ .GUARD_type = OME_MENU,
+ .onEnter = sacms_SetupTopMenu,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = saCmsMenuOfflineEntries,
+};
+
+#endif // CMS
+
+#endif // VTX_SMARTAUDIO
diff --git a/src/main/io/vtx_common.c b/src/main/io/vtx_string.c
similarity index 98%
rename from src/main/io/vtx_common.c
rename to src/main/io/vtx_string.c
index 5f89be8dd3..318c82b2e2 100644
--- a/src/main/io/vtx_common.c
+++ b/src/main/io/vtx_string.c
@@ -25,7 +25,7 @@
#include "platform.h"
#include "build/debug.h"
-#if defined(VTX_CONTROL)
+#if defined(VTX_COMMON)
const uint16_t vtx58FreqTable[5][8] =
{
diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_string.h
similarity index 92%
rename from src/main/io/vtx_common.h
rename to src/main/io/vtx_string.h
index cd593048b6..bbe8f4b0da 100644
--- a/src/main/io/vtx_common.h
+++ b/src/main/io/vtx_string.h
@@ -2,7 +2,7 @@
#include
-#if defined(VTX_CONTROL)
+#if defined(VTX_COMMON)
extern const uint16_t vtx58FreqTable[5][8];
extern const char * const vtx58BandNames[];
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 4e4716836a..343a215f45 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -35,7 +35,7 @@
#include "drivers/serial.h"
#include "drivers/system.h"
#include "io/vtx_tramp.h"
-#include "io/vtx_common.h"
+#include "io/vtx_string.h"
static serialPort_t *trampSerialPort = NULL;
diff --git a/src/main/target/common.h b/src/main/target/common.h
index b818ddf574..a6db3d0210 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -103,6 +103,7 @@
#define TELEMETRY_IBUS
#define USE_RX_MSP
#define USE_SERIALRX_JETIEXBUS
+#define VTX_COMMON
#define VTX_CONTROL
#define VTX_SMARTAUDIO
#define VTX_TRAMP
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index d881516c14..7a089ebff7 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -22,4 +22,7 @@
// Targets with built-in vtx do not need external vtx
#if defined(VTX) || defined(USE_RTC6705)
# undef VTX_CONTROL
+# undef VTX_COMMON
+# undef VTX_SMARTAUDIO
+# undef VTX_TRAMP
#endif