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;
}
/**