diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c
index 36fa60e340..cf213d57ee 100644
--- a/src/main/drivers/vtx_rtc6705.c
+++ b/src/main/drivers/vtx_rtc6705.c
@@ -30,7 +30,7 @@
#include "platform.h"
-#if defined(USE_VTX_RTC6705) && !defined(USE_VTX_RTC6705_SOFTSPI)
+#if defined(USE_VTX_RTC6705)
#include "common/maths.h"
@@ -38,7 +38,9 @@
#include "drivers/bus_spi_impl.h"
#include "drivers/io.h"
#include "drivers/time.h"
-#include "drivers/vtx_rtc6705.h"
+#include "drivers/vtx_rtc6705_soft_spi.h"
+
+#include "vtx_rtc6705.h"
#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
#define RTC6705_SET_R 400 //Reference clock
@@ -51,8 +53,7 @@
static IO_t vtxPowerPin = IO_NONE;
#endif
-static busDevice_t busInstance;
-static busDevice_t *busdev;
+static busDevice_t *busdev = NULL;
#define DISABLE_RTC6705() IOHi(busdev->busdev_u.spi.csnPin)
#define ENABLE_RTC6705() IOLo(busdev->busdev_u.spi.csnPin)
@@ -69,9 +70,6 @@ static busDevice_t *busdev;
#define RTC6705_RW_CONTROL_BIT (1 << 4)
#define RTC6705_ADDRESS (0x07)
-#define ENABLE_VTX_POWER() IOLo(vtxPowerPin)
-#define DISABLE_VTX_POWER() IOHi(vtxPowerPin)
-
/**
* Reverse a uint32_t (LSB to MSB)
* This is easier for when generating the frequency to then
@@ -93,42 +91,45 @@ static uint32_t reverse32(uint32_t in)
*/
bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
{
- busdev = &busInstance;
+ static busDevice_t busInstance;
- busdev->busdev_u.spi.csnPin = IOGetByTag(vtxIOConfig->csTag);
-
- bool rtc6705HaveRequiredResources =
- (busdev->busdev_u.spi.csnPin != IO_NONE);
-
-#ifdef RTC6705_POWER_PIN
- vtxPowerPin = IOGetByTag(vtxIOConfig->powerTag);
-
- rtc6705HaveRequiredResources = rtc6705HaveRequiredResources && vtxPowerPin;
-#endif
-
- if (!rtc6705HaveRequiredResources) {
+ IO_t csnPin = IOGetByTag(vtxIOConfig->csTag);
+ if (!csnPin) {
return false;
}
-#ifdef RTC6705_POWER_PIN
- IOInit(vtxPowerPin, OWNER_VTX_POWER, 0);
+ vtxPowerPin = IOGetByTag(vtxIOConfig->powerTag);
+ if (vtxPowerPin) {
+ IOInit(vtxPowerPin, OWNER_VTX_POWER, 0);
- DISABLE_VTX_POWER();
- IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
+ IOHi(vtxPowerPin);
+
+ IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
+ }
+
+ SPI_TypeDef *vtxSpiInstance = spiInstanceByDevice(SPI_CFG_TO_DEV(vtxIOConfig->spiDevice));
+ if (vtxSpiInstance) {
+ busdev = &busInstance;
+
+ busdev->busdev_u.spi.csnPin = csnPin;
+ IOInit(busdev->busdev_u.spi.csnPin, OWNER_VTX_CS, 0);
+
+ DISABLE_RTC6705();
+ // GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode.
+ // Note: It's critical to ensure that incorrect signals are not sent to the VTX.
+ IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
+
+ busdev->bustype = BUSTYPE_SPI;
+ spiBusSetInstance(busdev, vtxSpiInstance);
+
+ return true;
+#if defined(USE_VTX_RTC6705_SOFTSPI)
+ } else {
+ return rtc6705SoftSpiIOInit(vtxIOConfig, csnPin);
#endif
+ }
-
- IOInit(busdev->busdev_u.spi.csnPin, OWNER_VTX_CS, 0);
-
- DISABLE_RTC6705();
- // GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode.
- // Note: It's critical to ensure that incorrect signals are not sent to the VTX.
- IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
-
- busdev->bustype = BUSTYPE_SPI;
- spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(vtxIOConfig->spiDevice)));
-
- return true;
+ return false;;
}
/**
@@ -160,6 +161,14 @@ static void rtc6705Transfer(uint32_t command)
*/
void rtc6705SetFrequency(uint16_t frequency)
{
+#if defined(USE_VTX_RTC6705_SOFTSPI)
+ if (!busdev) {
+ rtc6705SoftSpiSetFrequency(frequency);
+
+ return;
+ }
+#endif
+
frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);
const uint32_t val_a = ((((uint64_t)frequency*(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)
@@ -178,30 +187,37 @@ void rtc6705SetFrequency(uint16_t frequency)
void rtc6705SetRFPower(uint8_t rf_power)
{
- rf_power = constrain(rf_power, VTX_RTC6705_MIN_POWER_VALUE, VTX_RTC6705_POWER_COUNT - 1);
+#if defined(USE_VTX_RTC6705_SOFTSPI)
+ if (!busdev) {
+ rtc6705SoftSpiSetRFPower(rf_power);
- spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_SLOW);
+ return;
+ }
+#endif
+
+ rf_power = constrain(rf_power, VTX_RTC6705_MIN_POWER_VALUE, VTX_RTC6705_POWER_COUNT - 1);
uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
val_hex |= RTC6705_ADDRESS; // address
const uint32_t data = rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK));
val_hex |= data << 5; // 4 address bits and 1 rw bit.
+ spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_SLOW);
+
rtc6705Transfer(val_hex);
}
void rtc6705Disable(void)
{
-#ifdef RTC6705_POWER_PIN
- DISABLE_VTX_POWER();
-#endif
+ if (vtxPowerPin) {
+ IOHi(vtxPowerPin);
+ }
}
void rtc6705Enable(void)
{
-#ifdef RTC6705_POWER_PIN
- ENABLE_VTX_POWER();
-#endif
+ if (vtxPowerPin) {
+ IOLo(vtxPowerPin);
+ }
}
-
#endif
diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c
index 99041890c8..09e29be908 100644
--- a/src/main/drivers/vtx_rtc6705_soft_spi.c
+++ b/src/main/drivers/vtx_rtc6705_soft_spi.c
@@ -23,7 +23,7 @@
#include "platform.h"
-#if defined(USE_VTX_RTC6705) && defined(USE_VTX_RTC6705_SOFTSPI)
+#if defined(USE_VTX_RTC6705_SOFTSPI)
#include "drivers/bus_spi.h"
#include "drivers/io.h"
@@ -41,53 +41,31 @@
#define PA_CONTROL_DEFAULT 0x4FBD
-#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin)
-#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin)
+#define RTC6705_SPICLK_ON() IOHi(rtc6705ClkPin)
+#define RTC6705_SPICLK_OFF() IOLo(rtc6705ClkPin)
-#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin)
-#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin)
+#define RTC6705_SPIDATA_ON() IOHi(rtc6705DataPin)
+#define RTC6705_SPIDATA_OFF() IOLo(rtc6705DataPin)
-#define DISABLE_RTC6705 IOHi(rtc6705CsnPin)
-#define ENABLE_RTC6705 IOLo(rtc6705CsnPin)
-
-#ifdef RTC6705_POWER_PIN
-static IO_t vtxPowerPin = IO_NONE;
-#endif
-
-#define ENABLE_VTX_POWER() IOLo(vtxPowerPin)
-#define DISABLE_VTX_POWER() IOHi(vtxPowerPin)
+#define DISABLE_RTC6705() IOHi(rtc6705CsnPin)
+#define ENABLE_RTC6705() IOLo(rtc6705CsnPin)
static IO_t rtc6705DataPin = IO_NONE;
static IO_t rtc6705CsnPin = IO_NONE;
static IO_t rtc6705ClkPin = IO_NONE;
-bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
+bool rtc6705SoftSpiIOInit(const vtxIOConfig_t *vtxIOConfig, const IO_t csnPin)
{
+ rtc6705CsnPin = csnPin;
+
rtc6705DataPin = IOGetByTag(vtxIOConfig->dataTag);
- rtc6705CsnPin = IOGetByTag(vtxIOConfig->csTag);
rtc6705ClkPin = IOGetByTag(vtxIOConfig->clockTag);
- bool rtc6705HaveRequiredResources = rtc6705DataPin && rtc6705CsnPin && rtc6705ClkPin;
-
-#ifdef RTC6705_POWER_PIN
- vtxPowerPin = IOGetByTag(vtxIOConfig->powerTag);
-
- rtc6705HaveRequiredResources &= (vtxPowerPin != IO_NONE);
-#endif
-
- if (!rtc6705HaveRequiredResources) {
+ if (!(rtc6705DataPin && rtc6705ClkPin)) {
return false;
}
-#ifdef RTC6705_POWER_PIN
- IOInit(vtxPowerPin, OWNER_VTX_POWER, 0);
-
- DISABLE_VTX_POWER();
- IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
-#endif
-
-
IOInit(rtc6705DataPin, OWNER_VTX_DATA, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP);
@@ -96,7 +74,7 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
// Important: The order of GPIO configuration calls are critical to ensure that incorrect signals are not briefly sent to the VTX.
// GPIO bit is enabled so here so the CS/LE pin output is not pulled low when the GPIO is set in output mode.
- DISABLE_RTC6705;
+ DISABLE_RTC6705();
IOInit(rtc6705CsnPin, OWNER_VTX_CS, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705CsnPin, IOCFG_OUT_PP);
@@ -105,42 +83,42 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
static void rtc6705_write_register(uint8_t addr, uint32_t data)
{
- ENABLE_RTC6705;
+ ENABLE_RTC6705();
delay(1);
// send address
for (int i = 0; i < 4; i++) {
if ((addr >> i) & 1) {
- RTC6705_SPIDATA_ON;
+ RTC6705_SPIDATA_ON();
} else {
- RTC6705_SPIDATA_OFF;
+ RTC6705_SPIDATA_OFF();
}
- RTC6705_SPICLK_ON;
+ RTC6705_SPICLK_ON();
delay(1);
- RTC6705_SPICLK_OFF;
+ RTC6705_SPICLK_OFF();
delay(1);
}
// Write bit
- RTC6705_SPIDATA_ON;
- RTC6705_SPICLK_ON;
+ RTC6705_SPIDATA_ON();
+ RTC6705_SPICLK_ON();
delay(1);
- RTC6705_SPICLK_OFF;
+ RTC6705_SPICLK_OFF();
delay(1);
for (int i = 0; i < 20; i++) {
if ((data >> i) & 1) {
- RTC6705_SPIDATA_ON;
+ RTC6705_SPIDATA_ON();
} else {
- RTC6705_SPIDATA_OFF;
+ RTC6705_SPIDATA_OFF();
}
- RTC6705_SPICLK_ON;
+ RTC6705_SPICLK_ON();
delay(1);
- RTC6705_SPICLK_OFF;
+ RTC6705_SPICLK_OFF();
delay(1);
}
- DISABLE_RTC6705;
+ DISABLE_RTC6705();
}
-void rtc6705SetFrequency(uint16_t channel_freq)
+void rtc6705SoftSpiSetFrequency(uint16_t channel_freq)
{
uint32_t freq = (uint32_t)channel_freq * 1000;
freq /= 40;
@@ -150,24 +128,8 @@ void rtc6705SetFrequency(uint16_t channel_freq)
rtc6705_write_register(1, (N << 7) | A);
}
-void rtc6705SetRFPower(uint8_t rf_power)
+void rtc6705SoftSpiSetRFPower(uint8_t rf_power)
{
rtc6705_write_register(7, (rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
}
-
-void rtc6705Disable(void)
-{
-#ifdef RTC6705_POWER_PIN
- DISABLE_VTX_POWER();
-#endif
-}
-
-void rtc6705Enable(void)
-{
-#ifdef RTC6705_POWER_PIN
- ENABLE_VTX_POWER();
-#endif
-}
-
-
#endif
diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.h b/src/main/drivers/vtx_rtc6705_soft_spi.h
new file mode 100644
index 0000000000..ce88c5815c
--- /dev/null
+++ b/src/main/drivers/vtx_rtc6705_soft_spi.h
@@ -0,0 +1,31 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include
+
+#include "pg/vtx_io.h"
+
+bool rtc6705SoftSpiIOInit(const vtxIOConfig_t *vtxIOConfig, const IO_t csnPin);
+void rtc6705SoftSpiSetFrequency(uint16_t freq);
+void rtc6705SoftSpiSetRFPower(uint8_t rf_power);
+
+
diff --git a/src/main/target/FISHDRONEF4/target.mk b/src/main/target/FISHDRONEF4/target.mk
index a40318066f..5009d92cbc 100644
--- a/src/main/target/FISHDRONEF4/target.mk
+++ b/src/main/target/FISHDRONEF4/target.mk
@@ -5,4 +5,5 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_mpu6500.c \
drivers/max7456.c \
+ drivers/vtx_rtc6705.c \
drivers/vtx_rtc6705_soft_spi.c
diff --git a/src/main/target/MOTOLABF4/target.mk b/src/main/target/MOTOLABF4/target.mk
index 75ca759a56..d837aa32a4 100644
--- a/src/main/target/MOTOLABF4/target.mk
+++ b/src/main/target/MOTOLABF4/target.mk
@@ -1,13 +1,12 @@
F405_TARGETS += $(TARGET)
FEATURES += SDCARD_SPI VCP
-ifeq ($(TARGET), MLTEMPF4)
TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/max7456.c
-else
-TARGET_SRC = \
- drivers/accgyro/accgyro_spi_mpu6000.c \
- drivers/max7456.c \
+
+ifeq ($(TARGET), MLTYPHF4)
+TARGET_SRC += \
+ drivers/vtx_rtc6705.c \
drivers/vtx_rtc6705_soft_spi.c
endif
diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h
index 49ce77df77..5d897733d1 100644
--- a/src/main/target/SPRACINGF3NEO/target.h
+++ b/src/main/target/SPRACINGF3NEO/target.h
@@ -99,7 +99,6 @@
#define SPI3_MOSI_PIN PB5
#define USE_VTX_RTC6705
-#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
#define RTC6705_CS_PIN PF4
#define RTC6705_SPI_INSTANCE SPI3
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index 2f06de7e3f..7d45f106be 100644
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -133,7 +133,6 @@
#if !defined(SPRACINGF4EVODG)
#define USE_VTX_RTC6705
-#define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control.
#define RTC6705_CS_PIN SPI3_NSS_PIN
#define RTC6705_SPI_INSTANCE SPI3
diff --git a/src/main/target/SPRACINGF4NEO/target.h b/src/main/target/SPRACINGF4NEO/target.h
index 3743b06884..f382eabeb2 100644
--- a/src/main/target/SPRACINGF4NEO/target.h
+++ b/src/main/target/SPRACINGF4NEO/target.h
@@ -141,7 +141,6 @@
// Bus Switched Device, Device B.
#define USE_VTX_RTC6705
-#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
#define RTC6705_CS_PIN PC4
#define RTC6705_SPI_INSTANCE SPI3
diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
index 8f7f21afa0..370717bbf8 100644
--- a/src/main/target/SPRACINGF7DUAL/target.h
+++ b/src/main/target/SPRACINGF7DUAL/target.h
@@ -145,7 +145,6 @@
#define USE_VTX_RTC6705
#define USE_VTX_RTC6705_SOFTSPI
-#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
#define RTC6705_SPI_MOSI_PIN PB0 // Shared with PWM8
#define RTC6705_CS_PIN PB6 // Shared with PWM5
diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk
index e756fd03e9..509ba8b763 100644
--- a/src/main/target/SPRACINGF7DUAL/target.mk
+++ b/src/main/target/SPRACINGF7DUAL/target.mk
@@ -12,6 +12,5 @@ TARGET_SRC = \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_lis3mdl.c \
drivers/max7456.c \
+ drivers/vtx_rtc6705.c \
drivers/vtx_rtc6705_soft_spi.c
-
-
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index b5e44cfe4a..d0b7026a40 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -24,10 +24,8 @@
#include "build/version.h"
-// Targets with built-in vtx do not need external vtx
-#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705_OPTIONAL)
-#undef USE_VTX_SMARTAUDIO
-#undef USE_VTX_TRAMP
+#if defined(USE_VTX_RTC6705_SOFTSPI)
+#define USE_VTX_RTC6705
#endif
#ifndef USE_DSHOT