diff --git a/make/source.mk b/make/source.mk index 99b2c6db53..b2499b3dc1 100644 --- a/make/source.mk +++ b/make/source.mk @@ -77,6 +77,7 @@ COMMON_SRC = \ pg/rx_pwm.c \ pg/sdcard.c \ pg/vcd.c \ + pg/usb.c \ scheduler/scheduler.c \ sensors/adcinternal.c \ sensors/battery.c \ diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index da7ffbfe1b..f423dbb1b5 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -30,8 +30,10 @@ #if defined(STM32F4) #include "usb_core.h" #include "usbd_cdc_vcp.h" -#ifdef USB_CDC_HID +#ifdef USE_USB_CDC_HID #include "usbd_hid_cdc_wrapper.h" +#include "pg/pg.h" +#include "pg/usb.h" #endif #include "usb_io.h" #elif defined(STM32F7) @@ -214,10 +216,14 @@ serialPort_t *usbVcpOpen(void) IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); -#ifdef USB_CDC_HID - USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_HID_CDC_cb, &USR_cb); -#else - USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); +#ifdef USE_USB_CDC_HID + if (usbDevice()->type == COMPOSITE) { + USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_HID_CDC_cb, &USR_cb); + } else { +#endif + USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); +#ifdef USE_USB_CDC_HID + } #endif #elif defined(STM32F7) usbGenerateDisconnectPulse(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index f6cfa98956..39fa1d7e9a 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -85,7 +85,7 @@ #include "telemetry/telemetry.h" -#ifdef USB_CDC_HID +#ifdef USE_USB_CDC_HID //TODO: Make it platform independent in the future #include "vcpf4/usbd_cdc_vcp.h" #include "usbd_hid_core.h" @@ -145,7 +145,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) isRXDataNew = true; -#ifdef USB_CDC_HID +#ifdef USE_USB_CDC_HID if (!ARMING_FLAG(ARMED)) { int8_t report[8]; for (int i = 0; i < 8; i++) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 359185f1ae..cafc278074 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -74,6 +74,7 @@ #include "pg/rx_pwm.h" #include "pg/sdcard.h" #include "pg/vcd.h" +#include "pg/usb.h" #include "rx/rx.h" #include "rx/cc2500_frsky_common.h" @@ -320,7 +321,7 @@ static const char * const lookupOverclock[] = { #endif #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } - + const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableOffOn), LOOKUP_TABLE_ENTRY(lookupTableUnit), @@ -954,6 +955,11 @@ const clivalue_t valueTable[] = { { "pinio_box", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = PINIO_COUNT, PG_PINIOBOX_CONFIG, offsetof(pinioBoxConfig_t, permanentId) }, #endif #endif + +//PG USB +#ifdef USE_USB_CDC_HID + { "usb_hid_cdc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_USB_CONFIG, offsetof(usbDev_t, type) }, +#endif }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index ddc101accb..416a48d114 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -121,7 +121,8 @@ #define PG_TRICOPTER_CONFIG 528 #define PG_PINIO_CONFIG 529 #define PG_PINIOBOX_CONFIG 530 -#define PG_BETAFLIGHT_END 530 +#define PG_USB_CONFIG 531 +#define PG_BETAFLIGHT_END 531 // OSD configuration (subject to change) diff --git a/src/main/pg/usb.c b/src/main/pg/usb.c new file mode 100644 index 0000000000..0442909582 --- /dev/null +++ b/src/main/pg/usb.c @@ -0,0 +1,24 @@ +/* + * 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 "platform.h" + +#include "pg/usb.h" +#include "pg/pg_ids.h" + +//Initialize to default - non composite +PG_REGISTER(usbDev_t, usbDevice, PG_USB_CONFIG, 0); diff --git a/src/main/pg/usb.h b/src/main/pg/usb.h new file mode 100644 index 0000000000..e0a4c8471b --- /dev/null +++ b/src/main/pg/usb.h @@ -0,0 +1,31 @@ +/* + * 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 "pg/pg.h" + +enum USB_DEV { + DEFAULT, + COMPOSITE +}; + +typedef struct usbDev_s { + uint8_t type; +} usbDev_t; + +PG_DECLARE(usbDev_t, usbDevice); diff --git a/src/main/target/WORMFC/target.h b/src/main/target/WORMFC/target.h index 43151e2811..bf96fc59e3 100644 --- a/src/main/target/WORMFC/target.h +++ b/src/main/target/WORMFC/target.h @@ -66,7 +66,6 @@ #define INVERTER_PIN_UART3 PB12 #define USE_VCP -#define USB_CDC_HID #define VBUS_SENSING_PIN PA9 #define VBUS_SENSING_ENABLED diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 0c4396d7c6..0c0378c96c 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -51,6 +51,7 @@ #define USE_GYRO_DATA_ANALYSE #define USE_ADC #define USE_ADC_INTERNAL +#define USE_USB_CDC_HID #define USE_USB_MSC #if defined(STM32F40_41xxx) || defined(STM32F411xE) diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index e117cbd7b5..660334bf4b 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -28,6 +28,9 @@ #include "platform.h" #include "build/version.h" +#include "pg/pg.h" +#include "pg/usb.h" + /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY * @{ */ @@ -52,11 +55,10 @@ #define USBD_VID 0x0483 -#ifdef USB_CDC_HID -#define USBD_PID 0x3256 -#else -#define USBD_PID 0x5740 +#ifdef USE_USB_CDC_HID +#define USBD_PID_COMPOSITE 0x3256 #endif +#define USBD_PID 0x5740 /** @defgroup USB_String_Descriptors * @{ @@ -120,9 +122,9 @@ USBD_DEVICE USR_desc = #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ -#ifdef USB_CDC_HID +#ifdef USE_USB_CDC_HID /* USB Standard Device Descriptor */ -__ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = +__ALIGN_BEGIN uint8_t USBD_DeviceDesc_Composite[USB_SIZ_DEVICE_DESC] __ALIGN_END = { 0x12, /*bLength */ USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ @@ -132,14 +134,15 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = 0x01, /*bDeviceProtocol*/ USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ LOBYTE(USBD_VID), HIBYTE(USBD_VID), /*idVendor*/ - LOBYTE(USBD_PID), HIBYTE(USBD_PID), /*idProduct*/ + LOBYTE(USBD_PID_COMPOSITE), + HIBYTE(USBD_PID_COMPOSITE), /*idProduct*/ 0x00, 0x02, /*bcdDevice rel. 2.00*/ USBD_IDX_MFC_STR, /*Index of manufacturer string*/ USBD_IDX_PRODUCT_STR, /*Index of product string*/ USBD_IDX_SERIAL_STR, /*Index of serial number string*/ USBD_CFG_MAX_NUM /*bNumConfigurations*/ } ; /* USB_DeviceDescriptor */ -#else +#endif /* USB Standard Device Descriptor */ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = { @@ -158,7 +161,6 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = USBD_IDX_SERIAL_STR, /*Index of serial number string*/ USBD_CFG_MAX_NUM /*bNumConfigurations*/ } ; /* USB_DeviceDescriptor */ -#endif #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ @@ -220,8 +222,14 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { (void)speed; - *length = sizeof(USBD_DeviceDesc); - return USBD_DeviceDesc; +#ifdef USE_USB_CDC_HID + if (usbDevice()->type == COMPOSITE) { + *length = sizeof(USBD_DeviceDesc_Composite); + return USBD_DeviceDesc_Composite; + } +#endif + *length = sizeof(USBD_DeviceDesc); + return USBD_DeviceDesc; } /**