diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 97ca2b619e..9e8fdc1fd2 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -60,6 +60,7 @@
#include "io/serial_cli.h"
#include "io/serial_msp.h"
#include "io/statusindicator.h"
+#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/msp.h"
diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c
index 916b22ba25..0eeead3156 100644
--- a/src/main/blackbox/blackbox_io.c
+++ b/src/main/blackbox/blackbox_io.c
@@ -36,6 +36,7 @@
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
+#include "io/vtx.h"
#include "io/gimbal.h"
#include "io/gps.h"
diff --git a/src/main/config/config.c b/src/main/config/config.c
index f7d1cb57a5..7a5d3d1539 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -54,6 +54,7 @@
#include "io/rc_curves.h"
#include "io/ledstrip.h"
#include "io/gps.h"
+#include "io/vtx.h"
#include "rx/rx.h"
@@ -567,6 +568,13 @@ static void resetConf(void)
masterConfig.ledstrip_visual_beeper = 0;
#endif
+#ifdef VTX
+ masterConfig.vtx_band = 4; //Fatshark/Airwaves
+ masterConfig.vtx_channel = 1; //CH1
+ masterConfig.vtx_mode = 0; //CH+BAND mode
+ masterConfig.vtx_mhz = 5740; //F0
+#endif
+
#ifdef SPRACINGF3
featureSet(FEATURE_BLACKBOX);
masterConfig.blackbox_device = 1;
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index ecc4af7ec8..ee3412202b 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -132,6 +132,15 @@ typedef struct master_t {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
+#ifdef VTX
+ uint8_t vtx_band; //1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband
+ uint8_t vtx_channel; //1-8
+ uint8_t vtx_mode; //0=ch+band 1=mhz
+ uint16_t vtx_mhz; //5740
+
+ vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
+#endif
+
#ifdef BLACKBOX
uint8_t blackbox_rate_num;
uint8_t blackbox_rate_denom;
diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c
new file mode 100644
index 0000000000..4bdaf9931d
--- /dev/null
+++ b/src/main/drivers/vtx_rtc6705.c
@@ -0,0 +1,191 @@
+/*
+ * 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 .
+ */
+
+/*
+ * Author: Giles Burgess (giles@multiflite.co.uk)
+ *
+ * This source code is provided as is and can be used/modified so long
+ * as this header is maintained with the file at all times.
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef VTX
+
+#include "common/maths.h"
+
+#include "drivers/vtx_rtc6705.h"
+#include "drivers/bus_spi.h"
+#include "drivers/system.h"
+
+#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
+#define RTC6705_SET_A1 0x8F3031 //5865
+#define RTC6705_SET_A2 0x8EB1B1 //5845
+#define RTC6705_SET_A3 0x8E3331 //5825
+#define RTC6705_SET_A4 0x8DB4B1 //5805
+#define RTC6705_SET_A5 0x8D3631 //5785
+#define RTC6705_SET_A6 0x8CB7B1 //5765
+#define RTC6705_SET_A7 0x8C4131 //5745
+#define RTC6705_SET_A8 0x8BC2B1 //5725
+#define RTC6705_SET_B1 0x8BF3B1 //5733
+#define RTC6705_SET_B2 0x8C6711 //5752
+#define RTC6705_SET_B3 0x8CE271 //5771
+#define RTC6705_SET_B4 0x8D55D1 //5790
+#define RTC6705_SET_B5 0x8DD131 //5809
+#define RTC6705_SET_B6 0x8E4491 //5828
+#define RTC6705_SET_B7 0x8EB7F1 //5847
+#define RTC6705_SET_B8 0x8F3351 //5866
+#define RTC6705_SET_E1 0x8B4431 //5705
+#define RTC6705_SET_E2 0x8AC5B1 //5685
+#define RTC6705_SET_E3 0x8A4731 //5665
+#define RTC6705_SET_E4 0x89D0B1 //5645
+#define RTC6705_SET_E5 0x8FA6B1 //5885
+#define RTC6705_SET_E6 0x902531 //5905
+#define RTC6705_SET_E7 0x90A3B1 //5925
+#define RTC6705_SET_E8 0x912231 //5945
+#define RTC6705_SET_F1 0x8C2191 //5740
+#define RTC6705_SET_F2 0x8CA011 //5760
+#define RTC6705_SET_F3 0x8D1691 //5780
+#define RTC6705_SET_F4 0x8D9511 //5800
+#define RTC6705_SET_F5 0x8E1391 //5820
+#define RTC6705_SET_F6 0x8E9211 //5840
+#define RTC6705_SET_F7 0x8F1091 //5860
+#define RTC6705_SET_F8 0x8F8711 //5880
+#define RTC6705_SET_R1 0x8A2151 //5658
+#define RTC6705_SET_R2 0x8B04F1 //5695
+#define RTC6705_SET_R3 0x8BF091 //5732
+#define RTC6705_SET_R4 0x8CD431 //5769
+#define RTC6705_SET_R5 0x8DB7D1 //5806
+#define RTC6705_SET_R6 0x8EA371 //5843
+#define RTC6705_SET_R7 0x8F8711 //5880
+#define RTC6705_SET_R8 0x9072B1 //5917
+
+#define RTC6705_SET_R 400 //Reference clock
+#define RTC6705_SET_FDIV 1024 //128*(fosc/1000000)
+#define RTC6705_SET_NDIV 16 //Remainder divider to get 'A' part of equation
+#define RTC6705_SET_WRITE 0x11 //10001b to write to register
+#define RTC6705_SET_DIVMULT 1000000 //Division value (to fit into a uint32_t) (Hz to MHz)
+
+#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
+#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
+
+// Define variables
+static const uint32_t channelArray[RTC6705_BAND_MAX][RTC6705_CHANNEL_MAX] = {
+ { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
+ { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
+ { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
+ { RTC6705_SET_F1, RTC6705_SET_F2, RTC6705_SET_F3, RTC6705_SET_F4, RTC6705_SET_F5, RTC6705_SET_F6, RTC6705_SET_F7, RTC6705_SET_F8 },
+ { RTC6705_SET_R1, RTC6705_SET_R2, RTC6705_SET_R3, RTC6705_SET_R4, RTC6705_SET_R5, RTC6705_SET_R6, RTC6705_SET_R7, RTC6705_SET_R8 },
+};
+
+/**
+ * Send a command and return if good
+ * TODO chip detect
+ */
+static bool rtc6705IsReady()
+{
+ // Sleep a little bit to make sure it has booted
+ delay(50);
+
+ // TODO Do a read and get current config (note this would be reading on MOSI (data) line)
+
+ return true;
+}
+
+/**
+ * Reverse a uint32_t (LSB to MSB)
+ * This is easier for when generating the frequency to then
+ * reverse the bits afterwards
+ */
+static uint32_t reverse32(uint32_t in) {
+ uint32_t out = 0;
+
+ for (uint8_t i = 0 ; i < 32 ; i++)
+ {
+ out |= ((in>>i) & 1)<<(31-i);
+ }
+
+ return out;
+}
+
+/**
+ * Start chip if available
+ */
+bool rtc6705Init()
+{
+ DISABLE_RTC6705;
+ spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
+ return rtc6705IsReady();
+}
+
+/**
+ * Transfer a 25bit packet to RTC6705
+ * This will just send it as a 32bit packet LSB meaning
+ * extra 0's get truncated on RTC6705 end
+ */
+static void rtc6705Transfer(uint32_t command)
+{
+ command = reverse32(command);
+
+ ENABLE_RTC6705;
+
+ spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 24) & 0xFF);
+ spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 16) & 0xFF);
+ spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 8) & 0xFF);
+ spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 0) & 0xFF);
+
+ DISABLE_RTC6705;
+}
+
+/**
+ * Set a band and channel
+ */
+void rtc6705SetChannel(uint8_t band, uint8_t channel)
+{
+ band = constrain(band, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
+ channel = constrain(channel, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
+
+ rtc6705Transfer(RTC6705_SET_HEAD);
+ rtc6705Transfer(channelArray[band-1][channel-1]);
+}
+
+ /**
+ * Set a freq in mhz
+ * Formula derived from datasheet
+ */
+void rtc6705SetFreq(uint16_t freq)
+{
+ freq = constrain(freq, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX);
+
+ uint32_t val_hex = 0;
+
+ uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
+ uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
+
+ val_hex |= RTC6705_SET_WRITE;
+ val_hex |= (val_a << 5);
+ val_hex |= (val_n << 12);
+
+ rtc6705Transfer(RTC6705_SET_HEAD);
+ rtc6705Transfer(val_hex);
+}
+
+#endif
diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h
new file mode 100644
index 0000000000..d72116659f
--- /dev/null
+++ b/src/main/drivers/vtx_rtc6705.h
@@ -0,0 +1,38 @@
+/*
+ * 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 .
+ */
+
+/*
+ * Author: Giles Burgess (giles@multiflite.co.uk)
+ *
+ * This source code is provided as is and can be used/modified so long
+ * as this header is maintained with the file at all times.
+ */
+
+#pragma once
+
+#include
+
+#define RTC6705_BAND_MIN 1
+#define RTC6705_BAND_MAX 5
+#define RTC6705_CHANNEL_MIN 1
+#define RTC6705_CHANNEL_MAX 8
+#define RTC6705_FREQ_MIN 5600
+#define RTC6705_FREQ_MAX 5950
+
+bool rtc6705Init();
+void rtc6705SetChannel(uint8_t band, uint8_t channel);
+void rtc6705SetFreq(uint16_t freq);
\ No newline at end of file
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 1448a6b5e1..f1ac9e3520 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -31,6 +31,7 @@
#include "sensors/sensors.h"
#include "io/statusindicator.h"
+#include "io/vtx.h"
#ifdef GPS
#include "io/gps.h"
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index cf85b7db37..afb78f3529 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -58,6 +58,7 @@
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/gps.h"
+#include "io/vtx.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index a72efd990e..8f25d6f6cb 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -49,6 +49,7 @@
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
+#include "io/vtx.h"
#include "io/display.h"
@@ -304,6 +305,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
#endif
+#ifdef VTX
+ if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
+ vtxIncrementBand();
+ }
+ if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
+ vtxDecrementBand();
+ }
+ if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
+ vtxIncrementChannel();
+ }
+ if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
+ vtxDecrementChannel();
+ }
+#endif
+
}
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 8cbb078a4d..0f107e403d 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -161,6 +161,7 @@ bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
+bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index ea6801e079..7cc6ae32f6 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -60,6 +60,7 @@
#include "io/flashfs.h"
#include "io/beeper.h"
#include "io/asyncfatfs/asyncfatfs.h"
+#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
@@ -164,6 +165,10 @@ static void cliFlashRead(char *cmdline);
#endif
#endif
+#ifdef VTX
+static void cliVtx(char *cmdline);
+#endif
+
#ifdef USE_SDCARD
static void cliSdInfo(char *cmdline);
#endif
@@ -326,6 +331,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
+#ifdef VTX
+ CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
+#endif
};
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@@ -768,6 +776,13 @@ const clivalue_t valueTable[] = {
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
#endif
+#ifdef VTX
+ { "vtx_band", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_band, .config.minmax = { 1, 5 } },
+ { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 1, 8 } },
+ { "vtx_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_mode, .config.minmax = { 0, 2 } },
+ { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } },
+#endif
+
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } },
@@ -1773,6 +1788,67 @@ static void cliFlashRead(char *cmdline)
#endif
#endif
+#ifdef VTX
+static void cliVtx(char *cmdline)
+{
+ int i, val = 0;
+ char *ptr;
+
+ if (isEmpty(cmdline)) {
+ // print out vtx channel settings
+ for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) {
+ vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i];
+ printf("vtx %u %u %u %u %u %u\r\n",
+ i,
+ cac->auxChannelIndex,
+ cac->band,
+ cac->channel,
+ MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep),
+ MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep)
+ );
+ }
+ } else {
+ ptr = cmdline;
+ i = atoi(ptr++);
+ if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) {
+ vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i];
+ uint8_t validArgumentCount = 0;
+ ptr = strchr(ptr, ' ');
+ if (ptr) {
+ val = atoi(++ptr);
+ if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) {
+ cac->auxChannelIndex = val;
+ validArgumentCount++;
+ }
+ }
+ ptr = strchr(ptr, ' ');
+ if (ptr) {
+ val = atoi(++ptr);
+ if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) {
+ cac->band = val;
+ validArgumentCount++;
+ }
+ }
+ ptr = strchr(ptr, ' ');
+ if (ptr) {
+ val = atoi(++ptr);
+ if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) {
+ cac->channel = val;
+ validArgumentCount++;
+ }
+ }
+ ptr = processChannelRangeArgs(ptr, &cac->range, &validArgumentCount);
+
+ if (validArgumentCount != 5) {
+ memset(cac, 0, sizeof(vtxChannelActivationCondition_t));
+ }
+ } else {
+ cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1);
+ }
+ }
+}
+#endif
+
static void dumpValues(uint16_t valueSection)
{
uint32_t i;
@@ -1963,6 +2039,12 @@ static void cliDump(char *cmdline)
}
#endif
+#ifdef VTX
+ cliPrint("\r\n# vtx\r\n");
+
+ cliVtx("");
+#endif
+
printSectionBreak();
dumpValues(MASTER_VALUE);
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index a65b224b86..f932fdb80a 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -57,6 +57,7 @@
#include "io/flashfs.h"
#include "io/transponder_ir.h"
#include "io/asyncfatfs/asyncfatfs.h"
+#include "io/vtx.h"
#include "telemetry/telemetry.h"
diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c
new file mode 100644
index 0000000000..3a48bc1a67
--- /dev/null
+++ b/src/main/io/vtx.c
@@ -0,0 +1,146 @@
+/*
+ * 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 "platform.h"
+
+#ifdef VTX
+
+#include "common/color.h"
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/compass.h"
+#include "drivers/system.h"
+#include "drivers/gpio.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_rx.h"
+#include "drivers/serial.h"
+
+#include "sensors/sensors.h"
+#include "sensors/gyro.h"
+#include "sensors/compass.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/boardalignment.h"
+#include "sensors/battery.h"
+
+#include "io/beeper.h"
+#include "io/serial.h"
+#include "io/gimbal.h"
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+#include "io/rc_curves.h"
+#include "io/ledstrip.h"
+#include "io/gps.h"
+#include "io/vtx.h"
+
+#include "rx/rx.h"
+
+#include "telemetry/telemetry.h"
+
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/failsafe.h"
+#include "flight/altitudehold.h"
+#include "flight/navigation.h"
+
+#include "config/config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+#include "config/runtime_config.h"
+
+static uint8_t locked = 0;
+
+void vtxInit()
+{
+ 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 vtxIncrementBand()
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
+}
+
+void vtxDecrementBand()
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
+}
+
+void vtxIncrementChannel()
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
+}
+
+void vtxDecrementChannel()
+{
+ setChannelSaveAndNotify(&(masterConfig.vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
+}
+
+void vtxUpdateActivatedChannel()
+{
+ 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
\ No newline at end of file
diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h
new file mode 100644
index 0000000000..ddcd7492de
--- /dev/null
+++ b/src/main/io/vtx.h
@@ -0,0 +1,40 @@
+/*
+ * 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 "drivers/vtx_rtc6705.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 vtxInit();
+void vtxIncrementBand();
+void vtxDecrementBand();
+void vtxIncrementChannel();
+void vtxDecrementChannel();
+void vtxUpdateActivatedChannel();
\ No newline at end of file
diff --git a/src/main/main.c b/src/main/main.c
index 0172dc2ebd..d89bb44923 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -70,6 +70,7 @@
#include "io/display.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/transponder_ir.h"
+#include "io/vtx.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
@@ -397,6 +398,10 @@ void init(void)
#endif
#endif
+#ifdef VTX
+ vtxInit();
+#endif
+
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
diff --git a/src/main/mw.c b/src/main/mw.c
index 54401519b6..c5c5e0af5f 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -65,7 +65,7 @@
#include "io/statusindicator.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/transponder_ir.h"
-
+#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/msp.h"
@@ -629,6 +629,9 @@ void processRx(void)
}
#endif
+#ifdef VTX
+ vtxUpdateActivatedChannel();
+#endif
}
void subTaskPidController(void)
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index 0f2f1d12f8..ad184b5aa6 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -62,6 +62,7 @@
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/beeper.h"
+#include "io/vtx.h"
#include "rx/rx.h"
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index f9060539bd..47685ab0a8 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -37,6 +37,7 @@
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/ledstrip.h"
+#include "io/vtx.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"