diff --git a/make/source.mk b/make/source.mk
index 4a80c8e5b4..9c074650ed 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -51,6 +51,7 @@ COMMON_SRC = \
io/serial.c \
io/statusindicator.c \
io/transponder_ir.c \
+ io/usb_cdc_hid.c \
msp/msp_serial.c \
scheduler/scheduler.c \
sensors/adcinternal.c \
@@ -297,6 +298,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
io/serial_4way_avrootloader.c \
io/serial_4way_stk500v2.c \
io/transponder_ir.c \
+ io/usb_cdc_hid.c \
msp/msp_serial.c \
cms/cms.c \
cms/cms_menu_blackbox.c \
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 4b781fc0a8..cf52d8bf3e 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -69,6 +69,7 @@
#include "io/transponder_ir.h"
#include "io/vtx_tramp.h" // Will be gone
#include "io/rcdevice_cam.h"
+#include "io/usb_cdc_hid.h"
#include "io/vtx.h"
#include "msp/msp_serial.h"
@@ -91,17 +92,6 @@
#include "telemetry/telemetry.h"
-#ifdef USE_USB_CDC_HID
-//TODO: Make it platform independent in the future
-#ifdef STM32F4
-#include "vcpf4/usbd_cdc_vcp.h"
-#include "usbd_hid_core.h"
-#elif defined(STM32F7)
-#include "usbd_cdc_interface.h"
-#include "usbd_hid.h"
-#endif
-#endif
-
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
@@ -161,16 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
#ifdef USE_USB_CDC_HID
if (!ARMING_FLAG(ARMED)) {
- int8_t report[8];
- for (int i = 0; i < 8; i++) {
- report[i] = scaleRange(constrain(rcData[i], 1000, 2000), 1000, 2000, -127, 127);
- }
-#ifdef STM32F4
- USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report));
-#elif defined(STM32F7)
- extern USBD_HandleTypeDef USBD_Device;
- USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report));
-#endif
+ sendRcDataToHid();
}
#endif
diff --git a/src/main/io/usb_cdc_hid.c b/src/main/io/usb_cdc_hid.c
new file mode 100644
index 0000000000..d4c6957114
--- /dev/null
+++ b/src/main/io/usb_cdc_hid.c
@@ -0,0 +1,70 @@
+/*
+ * 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 "platform.h"
+
+#ifdef USE_USB_CDC_HID
+
+#include "common/maths.h"
+
+#include "rx/rx.h"
+
+//TODO: Make it platform independent in the future
+#ifdef STM32F4
+#include "vcpf4/usbd_cdc_vcp.h"
+#include "usbd_hid_core.h"
+#elif defined(STM32F7)
+#include "usbd_cdc_interface.h"
+#include "usbd_hid.h"
+#endif
+
+#define USB_CDC_HID_NUM_AXES 8
+
+// In the windows joystick driver, the axes are defined as shown in the third column.
+
+const uint8_t hidChannelMapping[] = {
+ 0, // X, ROLL
+ 1, // Y, PITCH
+ 6, // , AUX3
+ 3, // Y Rotation, THROTTLE
+ 4, // Z Rotation, AUX1
+ 2, // X Rotation, YAW
+ 7, // , AUX4
+ 5, // Wheel, AUX2
+};
+
+void sendRcDataToHid(void)
+{
+ int8_t report[8];
+ for (unsigned i = 0; i < USB_CDC_HID_NUM_AXES; i++) {
+ const uint8_t channel = getMappedChannel(hidChannelMapping[i]);
+ report[i] = scaleRange(constrain(rcData[channel], 1000, 2000), 1000, 2000, -127, 127);
+ if (i == 1 || i == 2) {
+ report[i] = -report[i];
+ }
+ }
+#ifdef STM32F4
+ USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report));
+#elif defined(STM32F7)
+ extern USBD_HandleTypeDef USBD_Device;
+ USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report));
+#endif
+}
+#endif
diff --git a/src/main/io/usb_cdc_hid.h b/src/main/io/usb_cdc_hid.h
new file mode 100644
index 0000000000..ff0b538862
--- /dev/null
+++ b/src/main/io/usb_cdc_hid.h
@@ -0,0 +1,23 @@
+/*
+ * 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
+
+void sendRcDataToHid(void);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index f565249d84..37175eb6ab 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -466,11 +466,16 @@ STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rx
return sample;
}
+uint8_t getMappedChannel(uint8_t channel)
+{
+ return channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
+}
+
static void readRxChannelsApplyRanges(void)
{
for (int channel = 0; channel < rxChannelCount; channel++) {
- const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
+ const uint8_t rawChannel = getMappedChannel(channel);
// sample the channel
uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel);
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index cabe221433..2b843e7ae5 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -174,3 +174,4 @@ void resumeRxSignal(void);
uint16_t rxGetRefreshRate(void);
+uint8_t getMappedChannel(uint8_t channel);