diff --git a/Makefile b/Makefile index 61b52ec0fa..27eccdd7d2 100644 --- a/Makefile +++ b/Makefile @@ -741,6 +741,7 @@ SPRACINGF3MINI_SRC = \ drivers/sdcard_standard.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ + drivers/usb_detection.c \ io/asyncfatfs/asyncfatfs.c \ io/asyncfatfs/fat_standard.c \ io/transponder_ir.c \ diff --git a/src/main/drivers/usb_detection.c b/src/main/drivers/usb_detection.c new file mode 100644 index 0000000000..f3e0243052 --- /dev/null +++ b/src/main/drivers/usb_detection.c @@ -0,0 +1,63 @@ +/* + * 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 "sdcard.h" + +#include +#include +#include + +#include "platform.h" + +#include "gpio.h" + +#include "drivers/system.h" + +void usbCableDetectDeinit(void) +{ +#ifdef USB_DETECT_PIN + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_InitStructure.GPIO_Pin = USB_DETECT_PIN; + GPIO_Init(USB_DETECT_GPIO_PORT, &GPIO_InitStructure); +#endif +} + +void usbCableDetectInit(void) +{ +#ifdef USB_DETECT_PIN + RCC_AHBPeriphClockCmd(USB_DETECT_GPIO_CLK, ENABLE); + + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_InitStructure.GPIO_Pin = USB_DETECT_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(USB_DETECT_GPIO_PORT, &GPIO_InitStructure); +#endif +} + +bool usbCableIsInserted(void) +{ + bool result = false; + +#ifdef USB_DETECT_PIN + result = (GPIO_ReadInputData(USB_DETECT_GPIO_PORT) & USB_DETECT_PIN) != 0; +#endif + + return result; +} diff --git a/src/main/drivers/usb_detection.h b/src/main/drivers/usb_detection.h new file mode 100644 index 0000000000..5a3c53d6b4 --- /dev/null +++ b/src/main/drivers/usb_detection.h @@ -0,0 +1,22 @@ +/* + * 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 + +void usbCableDetectDeinit(void); +void usbCableDetectInit(void); +bool usbCableIsInserted(void); diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index 4527697c2d..b30d0c7423 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -28,6 +28,8 @@ #include "drivers/transponder_ir.h" #include "drivers/system.h" +#include "drivers/usb_detection.h" + #include "io/transponder_ir.h" #include "config/config.h" @@ -63,6 +65,13 @@ void updateTransponder(void) nextUpdateAt = now + 4500 + jitter; +#ifdef SPRACINGF3MINI + // reduce current draw when USB cable is plugged in by decreasing the transponder transmit rate. + if (usbCableIsInserted()) { + nextUpdateAt = now + (1000 * 1000) / 10; // 10 hz. + } +#endif + transponderIrTransmit(); } diff --git a/src/main/main.c b/src/main/main.c index 0050d971c5..59fbe81bcf 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -128,6 +128,7 @@ void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); void transponderInit(uint8_t* transponderCode); +void usbCableDetectInit(void); #ifdef STM32F303xC // from system_stm32f30x.c @@ -522,6 +523,10 @@ void init(void) } #endif +#ifdef USB_CABLE_DETECTION + usbCableDetectInit(); +#endif + #ifdef TRANSPONDER transponderInit(masterConfig.transponderData); transponderEnable(); diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 36933f0bc3..6ea0d82d9f 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -194,6 +194,11 @@ #define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 #define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define USB_CABLE_DETECTION +#define USB_DETECT_PIN GPIO_Pin_5 +#define USB_DETECT_GPIO_PORT GPIOB +#define USB_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOC + #define GPS #define BLACKBOX #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT