diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 23dd4a47b2..bed23bdbd4 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4525,6 +4525,12 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_USB_DETECT DEFS( OWNER_USB_DETECT, PG_USB_CONFIG, usbDev_t, detectPin ), #endif +#ifdef USE_VTX_RTC6705 + DEFS( OWNER_VTX_POWER, PG_VTX_IO_CONFIG, vtxIOConfig_t, powerTag ), + DEFS( OWNER_VTX_CS, PG_VTX_IO_CONFIG, vtxIOConfig_t, csTag ), + DEFS( OWNER_VTX_DATA, PG_VTX_IO_CONFIG, vtxIOConfig_t, dataTag ), + DEFS( OWNER_VTX_CLK, PG_VTX_IO_CONFIG, vtxIOConfig_t, clockTag ), +#endif }; #undef DEFS diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index bf5f26d13e..33660bb171 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -84,6 +84,7 @@ #include "pg/rx_spi_cc2500.h" #include "pg/sdcard.h" #include "pg/vcd.h" +#include "pg/vtx_io.h" #include "pg/usb.h" #include "pg/sdio.h" #include "pg/rcdevice.h" @@ -1292,6 +1293,11 @@ const clivalue_t valueTable[] = { { "vtx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) }, #endif +// PG_VTX_IO +#ifdef USE_VTX_RTC6705 + { "vtx_spi_bus", VAR_UINT8 | HARDWARE_VALUE | MASTER_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_VTX_IO_CONFIG, offsetof(vtxIOConfig_t, spiDevice) }, +#endif + // PG_VCD_CONFIG #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VIDEO_SYSTEM }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) }, diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 45bf5ab926..ceabba8a9a 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -66,7 +66,10 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "INVERTER", "LED_STRIP", "TRANSPONDER", - "VTX", + "VTX_POWER", + "VTX_CS", + "VTX_DATA", + "VTX_CLK", "COMPASS_CS", "RX_BIND_PLUG", "ESCSERIAL", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 71ba9475a9..233e74871d 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -64,7 +64,10 @@ typedef enum { OWNER_INVERTER, OWNER_LED_STRIP, OWNER_TRANSPONDER, - OWNER_VTX, + OWNER_VTX_POWER, + OWNER_VTX_CS, + OWNER_VTX_DATA, + OWNER_VTX_CLK, OWNER_COMPASS_CS, OWNER_RX_BIND_PLUG, OWNER_ESCSERIAL, diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 4aaa960206..b583821d0b 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -39,7 +39,6 @@ #include "drivers/time.h" #include "drivers/vtx_rtc6705.h" - #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_R 400 //Reference clock #define RTC6705_SET_FDIV 1024 //128*(fosc/1000000) @@ -50,16 +49,18 @@ #ifdef RTC6705_POWER_PIN static IO_t vtxPowerPin = IO_NONE; #endif -static IO_t vtxCSPin = IO_NONE; -#define DISABLE_RTC6705() IOHi(vtxCSPin) +static busDevice_t busInstance; +static busDevice_t *busdev; + +#define DISABLE_RTC6705() IOHi(busdev->busdev_u.spi.csnPin) #ifdef USE_RTC6705_CLK_HACK static IO_t vtxCLKPin = IO_NONE; // HACK for missing pull up on CLK line - drive the CLK high *before* enabling the CS pin. -#define ENABLE_RTC6705() {IOHi(vtxCLKPin); delayMicroseconds(5); IOLo(vtxCSPin); } +#define ENABLE_RTC6705() {IOHi(vtxCLKPin); delayMicroseconds(5); IOLo(busdev->busdev_u.spi.csnPin); } #else -#define ENABLE_RTC6705() IOLo(vtxCSPin) +#define ENABLE_RTC6705() IOLo(busdev->busdev_u.spi.csnPin) #endif #define DP_5G_MASK 0x7000 // b111000000000000 @@ -77,7 +78,6 @@ static IO_t vtxCLKPin = IO_NONE; #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 @@ -97,28 +97,51 @@ static uint32_t reverse32(uint32_t in) /** * Start chip if available */ -void rtc6705IOInit(void) +bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig) { + busdev = &busInstance; + + busdev->busdev_u.spi.csnPin = IOGetByTag(vtxIOConfig->csTag); + + bool rtc6705HaveRequiredResources = + (busdev->busdev_u.spi.csnPin != IO_NONE); + #ifdef RTC6705_POWER_PIN - vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN)); - IOInit(vtxPowerPin, OWNER_VTX, 0); + vtxPowerPin = IOGetByTag(vtxIOConfig->powerTag); + + rtc6705HaveRequiredResources = rtc6705HaveRequiredResources && vtxPowerPin; +#endif + +#ifdef USE_RTC6705_CLK_HACK + vtxCLKPin = IOGetByTag(vtxIOConfig->clockTag); + // we assume the CLK pin will have been initialised by the SPI code. + + rtc6705HaveRequiredResources = rtc6705HaveRequiredResources && vtxCLKPin; +#endif + + if (!rtc6705HaveRequiredResources) { + return false; + } + +#ifdef RTC6705_POWER_PIN + IOInit(vtxPowerPin, OWNER_VTX_POWER, 0); DISABLE_VTX_POWER(); IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP); #endif -#ifdef USE_RTC6705_CLK_HACK - vtxCLKPin = IOGetByTag(IO_TAG(RTC6705_CLK_PIN)); - // we assume the CLK pin will have been initialised by the SPI code. -#endif - vtxCSPin = IOGetByTag(IO_TAG(RTC6705_CS_PIN)); - IOInit(vtxCSPin, OWNER_VTX, 0); + 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(vtxCSPin, IOCFG_OUT_PP); + IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); + + busdev->bustype = BUSTYPE_SPI; + spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(vtxIOConfig->spiDevice))); + + return true; } /** @@ -132,10 +155,10 @@ static void rtc6705Transfer(uint32_t 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); + spiTransferByte(busdev->busdev_u.spi.instance, (command >> 24) & 0xFF); + spiTransferByte(busdev->busdev_u.spi.instance, (command >> 16) & 0xFF); + spiTransferByte(busdev->busdev_u.spi.instance, (command >> 8) & 0xFF); + spiTransferByte(busdev->busdev_u.spi.instance, (command >> 0) & 0xFF); delayMicroseconds(2); @@ -159,7 +182,7 @@ void rtc6705SetFrequency(uint16_t frequency) val_hex |= (val_a << 5); val_hex |= (val_n << 12); - spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); + spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_SLOW); rtc6705Transfer(RTC6705_SET_HEAD); delayMicroseconds(10); @@ -170,7 +193,7 @@ void rtc6705SetRFPower(uint8_t rf_power) { rf_power = constrain(rf_power, VTX_RTC6705_MIN_POWER, VTX_RTC6705_POWER_COUNT - 1); - spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); + spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_SLOW); uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write val_hex |= RTC6705_ADDRESS; // address diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h index d11561c101..b3acb18e33 100644 --- a/src/main/drivers/vtx_rtc6705.h +++ b/src/main/drivers/vtx_rtc6705.h @@ -29,6 +29,8 @@ #include +#include "pg/vtx_io.h" + #define VTX_RTC6705_BAND_COUNT 5 #define VTX_RTC6705_CHANNEL_COUNT 8 #define VTX_RTC6705_POWER_COUNT 3 @@ -45,8 +47,10 @@ #define VTX_RTC6705_BOOT_DELAY 350 // milliseconds -void rtc6705IOInit(void); +bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig); void rtc6705SetFrequency(uint16_t freq); void rtc6705SetRFPower(uint8_t rf_power); void rtc6705Disable(void); void rtc6705Enable(void); + + diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c index 0b67c25142..99041890c8 100644 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -31,6 +31,7 @@ #include "drivers/time.h" #include "drivers/vtx_rtc6705.h" + #define DP_5G_MASK 0x7000 #define PA5G_BS_MASK 0x0E00 #define PA5G_PW_MASK 0x0180 @@ -60,31 +61,46 @@ static IO_t rtc6705DataPin = IO_NONE; static IO_t rtc6705CsnPin = IO_NONE; static IO_t rtc6705ClkPin = IO_NONE; -void rtc6705IOInit(void) + +bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig) { + rtc6705DataPin = IOGetByTag(vtxIOConfig->dataTag); + rtc6705CsnPin = IOGetByTag(vtxIOConfig->csTag); + rtc6705ClkPin = IOGetByTag(vtxIOConfig->clockTag); + + bool rtc6705HaveRequiredResources = rtc6705DataPin && rtc6705CsnPin && rtc6705ClkPin; + #ifdef RTC6705_POWER_PIN - vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN)); - IOInit(vtxPowerPin, OWNER_VTX, 0); + vtxPowerPin = IOGetByTag(vtxIOConfig->powerTag); + + rtc6705HaveRequiredResources &= (vtxPowerPin != IO_NONE); +#endif + + if (!rtc6705HaveRequiredResources) { + return false; + } + +#ifdef RTC6705_POWER_PIN + IOInit(vtxPowerPin, OWNER_VTX_POWER, 0); DISABLE_VTX_POWER(); IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP); #endif - rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPI_MOSI_PIN)); - rtc6705CsnPin = IOGetByTag(IO_TAG(RTC6705_CS_PIN)); - rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); - IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); + IOInit(rtc6705DataPin, OWNER_VTX_DATA, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); - IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET); + IOInit(rtc6705ClkPin, OWNER_VTX_CLK, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); // 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; - IOInit(rtc6705CsnPin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET); + IOInit(rtc6705CsnPin, OWNER_VTX_CS, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705CsnPin, IOCFG_OUT_PP); + + return true; } static void rtc6705_write_register(uint8_t addr, uint32_t data) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index f5936e92c7..5cb017b325 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -113,6 +113,7 @@ #include "pg/rx_pwm.h" #include "pg/sdcard.h" #include "pg/vcd.h" +#include "pg/vtx_io.h" #include "rx/rx.h" #include "rx/rx_spi.h" @@ -491,7 +492,7 @@ void init(void) #endif #ifdef USE_VTX_RTC6705 - rtc6705IOInit(); + bool useRTC6705 = rtc6705IOInit(vtxIOConfig()); #endif #ifdef USE_CAMERA_CONTROL @@ -734,10 +735,7 @@ void init(void) #endif #ifdef USE_VTX_RTC6705 -#ifdef VTX_RTC6705_OPTIONAL - if (!vtxCommonDevice()) // external VTX takes precedence when configured. -#endif - { + if (!vtxCommonDevice() && useRTC6705) { // external VTX takes precedence when configured. vtxRTC6705Init(); } #endif diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 21c349a0b2..2347d554e1 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -80,6 +80,7 @@ //#define PG_VTX_CONFIG 54 // CF 1.x #define PG_GPS_RESCUE 55 // struct OK #define PG_POSITION 56 +#define PG_VTX_IO_CONFIG 57 // Driver configuration #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight diff --git a/src/main/pg/vtx_io.c b/src/main/pg/vtx_io.c new file mode 100644 index 0000000000..74112296ed --- /dev/null +++ b/src/main/pg/vtx_io.c @@ -0,0 +1,74 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_VTX_RTC6705 + +#include "drivers/bus_spi.h" +#include "drivers/io.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "vtx_io.h" + +PG_REGISTER_WITH_RESET_FN(vtxIOConfig_t, vtxIOConfig, PG_VTX_IO_CONFIG, 0); + +void pgResetFn_vtxIOConfig(vtxIOConfig_t *vtxIOConfig) +{ + // common + +#ifdef RTC6705_CS_PIN + vtxIOConfig->csTag = IO_TAG(RTC6705_CS_PIN); +#else + vtxIOConfig->csTag = IO_TAG_NONE; +#endif + +#ifdef RTC6705_POWER_PIN + vtxIOConfig->powerTag = IO_TAG(RTC6705_POWER_PIN); +#else + vtxIOConfig->powerTag = IO_TAG_NONE; +#endif + + // software SPI + +#ifdef RTC6705_SPICLK_PIN + vtxIOConfig->clockTag = IO_TAG(RTC6705_SPICLK_PIN); +#else + vtxIOConfig->clockTag = IO_TAG_NONE; +#endif + +#ifdef RTC6705_SPI_MOSI_PIN + vtxIOConfig->dataTag = IO_TAG(RTC6705_SPI_MOSI_PIN); +#else + vtxIOConfig->dataTag = IO_TAG_NONE; +#endif + + // hardware spi + +#ifdef RTC6705_SPI_INSTANCE + vtxIOConfig->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(RTC6705_SPI_INSTANCE)); +#endif +} +#endif diff --git a/src/main/pg/vtx_io.h b/src/main/pg/vtx_io.h new file mode 100644 index 0000000000..9eb9465367 --- /dev/null +++ b/src/main/pg/vtx_io.h @@ -0,0 +1,42 @@ +/* + * 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 "drivers/io_types.h" + +#include "pg/pg.h" + +typedef struct vtxIOConfig_s { + // common settings for both hardware and software SPI + ioTag_t csTag; + ioTag_t powerTag; + + // settings for software SPI only + ioTag_t dataTag; + ioTag_t clockTag; + + // settings for hardware SPI only + uint8_t spiDevice; +} vtxIOConfig_t; + +PG_DECLARE(vtxIOConfig_t, vtxIOConfig);