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"