Projectkk2glider/issue 4479 slow joystick (#4569)

* Missing debug timer

* USB driver cleanup

* Re #4479: Joystick update called from mixer thread every 10ms

* Cosmetics

* Compilation fix

* Cosmetics

* Another compilation fix

radio/src/debug.cpp

@@ -240,6 +240,7 @@ const char * const debugTimerNames[DEBUG_TIMERS_COUNT] = {
   ,"Mixer calc "   // debugTimerMixer,
   ,"Tel. wakeup"   // debugTimerTelemetryWakeup,
   ,"perMain dur"   // debugTimerPerMain,
+  ," period    "   // debugTimerPerMainPeriod,
   ," perMain s1"   // debugTimerPerMain1,
   ," guiMain   "   // debugTimerGuiMain,
   ,"  LUA bg   "   // debugTimerLuaBg,

radio/src/debug.h

@@ -366,6 +366,7 @@ enum DebugTimers {
   debugTimerMixer,
   debugTimerTelemetryWakeup,
   debugTimerPerMain,
+  debugTimerPerMainPeriod,
   debugTimerPerMain1,
   debugTimerGuiMain,
   debugTimerLuaBg,

radio/src/main_arm.cpp

@@ -27,30 +27,19 @@ uint8_t mainRequestFlags = 0;
 void handleUsbConnection()
 {
 #if defined(STM32) && !defined(SIMU)
-  static bool usbStarted = false;
+  if (!usbStarted() && usbPlugged()) {
-
-  if (!usbStarted && usbPlugged()) {
-    usbStarted = true;
     usbStart();
 #if defined(USB_MASS_STORAGE)
     opentxClose(false);
     usbPluggedIn();
 #endif
   }
-  if (usbStarted && !usbPlugged()) {
+  if (usbStarted() && !usbPlugged()) {
-    usbStarted = false;
     usbStop();
 #if defined(USB_MASS_STORAGE) && !defined(EEPROM)
     opentxResume();
 #endif
   }
-
-#if defined(USB_JOYSTICK)
-  if (usbStarted ) {
-    usbJoystickUpdate();
-  }
-#endif
-
 #endif // defined(STM32) && !defined(SIMU)
 }
 

radio/src/targets/common/arm/stm32/CMakeLists.txt

@@ -36,7 +36,7 @@ set(FIRMWARE_TARGET_SRC
   ../common/arm/stm32/usb_bsp.c
   ../common/arm/stm32/usbd_desc.c
   ../common/arm/stm32/usbd_usr.cpp
-  ../common/arm/stm32/usb_driver.c
+  ../common/arm/stm32/usb_driver.cpp
   )
 if(USB STREQUAL SERIAL)
   add_definitions(-DUSB_SERIAL)

radio/src/targets/common/arm/stm32/usb_driver.cpp

@@ -18,12 +18,21 @@
  * GNU General Public License for more details.
  */
 
-#include "board.h"
+#if defined(__cplusplus)
+extern "C" {
+#endif
 #include "usb_dcd_int.h"
 #include "usb_bsp.h"
+#if defined(__cplusplus)
+}
+#endif
+
+#include "opentx.h"
 #include "debug.h"
 
-int usbPlugged(void)
+static bool usbDriverStarted = false;
+
+int usbPlugged()
 {
   // debounce
   static uint8_t debounced_state = 0;
@@ -46,19 +55,20 @@ int usbPlugged(void)
 
 USB_OTG_CORE_HANDLE USB_OTG_dev;
 
-void OTG_FS_IRQHandler(void)
+extern "C" void OTG_FS_IRQHandler()
 {
   DEBUG_INTERRUPT(INT_OTG_FS);
   USBD_OTG_ISR_Handler(&USB_OTG_dev);
 }
 
-void usbInit(void)
+void usbInit()
 {
   // Initialize hardware
   USB_OTG_BSP_Init(&USB_OTG_dev);
+  usbDriverStarted = false;
 }
 
-void usbStart(void)
+void usbStart()
 {
 #if defined(USB_JOYSTICK)
   // initialize USB as HID device
@@ -70,9 +80,60 @@ void usbStart(void)
   // initialize USB as MSC device
   USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_MSC_cb, &USR_cb);
 #endif
+  usbDriverStarted = true;
 }
 
-void usbStop(void)
+void usbStop()
 {
+  usbDriverStarted = false;
   USBD_DeInit(&USB_OTG_dev);
 }
+
+uint8_t usbStarted()
+{
+  return usbDriverStarted;
+}
+
+#if defined(USB_JOYSTICK)
+/*
+  Prepare and send new USB data packet
+
+  The format of HID_Buffer is defined by
+  USB endpoint description can be found in
+  file usb_hid_joystick.c, variable HID_JOYSTICK_ReportDesc
+*/
+void usbJoystickUpdate()
+{
+  static uint8_t HID_Buffer[HID_IN_PACKET];
+
+  // test to se if TX buffer is free
+  if (USBD_HID_SendReport(&USB_OTG_dev, 0, 0) == USBD_OK) {
+    //buttons
+    HID_Buffer[0] = 0;
+    HID_Buffer[1] = 0;
+    HID_Buffer[2] = 0;
+    for (int i = 0; i < 8; ++i) {
+      if ( channelOutputs[i+8] > 0 ) {
+        HID_Buffer[0] |= (1 << i);
+      }
+      if ( channelOutputs[i+16] > 0 ) {
+        HID_Buffer[1] |= (1 << i);
+      }
+      if ( channelOutputs[i+24] > 0 ) {
+        HID_Buffer[2] |= (1 << i);
+      }
+    }
+
+    //analog values
+    //uint8_t * p = HID_Buffer + 1;
+    for (int i = 0; i < 8; ++i) {
+      int16_t value = channelOutputs[i] / 8;
+      if ( value > 127 ) value = 127;
+      else if ( value < -127 ) value = -127;
+      HID_Buffer[i+3] = static_cast<int8_t>(value);
+    }
+    USBD_HID_SendReport(&USB_OTG_dev, HID_Buffer, HID_IN_PACKET);
+  }
+}
+
+#endif // #defined(USB_JOYSTICK)

radio/src/targets/common/arm/stm32/usbd_hid_joystick.c

@@ -1,23 +1,23 @@
-/*
+/*
- * Copyright (C) OpenTX
+ * Copyright (C) OpenTX
- *
+ *
- * Based on code named
+ * Based on code named
- *   th9x - http://code.google.com/p/th9x 
+ *   th9x - http://code.google.com/p/th9x 
- *   er9x - http://code.google.com/p/er9x
+ *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
- *
+ *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
+ *
- * This program is free software; you can redistribute it and/or modify
+ * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
+ * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
+ * published by the Free Software Foundation.
- *
+ *
- * This program is distributed in the hope that it will be useful,
+ * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * GNU General Public License for more details.
- */
+ */
-
+
 /* Includes ------------------------------------------------------------------*/
 #include "usbd_desc.h"
 #include "usbd_hid_core.h"
@@ -385,25 +385,24 @@ static uint8_t  USBD_HID_Setup (void  *pdev,
 
 
 /**
-  * @brief  USBD_HID_SendReport 
+  * @brief  USBD_HID_SendReport
-  *         Send HID Report
+  *         Send HID Report if TX buffer is free and USB device is configured.
   * @param  pdev: device instance
-  * @param  buff: pointer to report
+  * @param  buff: pointer to report, if this parameter is NULL then function just test if new buffer can be sent
   * @retval status
   */
-uint8_t USBD_HID_SendReport     (USB_OTG_CORE_HANDLE  *pdev, 
+uint8_t USBD_HID_SendReport(USB_OTG_CORE_HANDLE  *pdev, uint8_t * report, uint16_t len)
-                                 uint8_t *report,
-                                 uint16_t len)
 {
-  if (pdev->dev.device_status == USB_OTG_CONFIGURED )
+  if (pdev->dev.device_status == USB_OTG_CONFIGURED) {
-  {
+    if (ReportSent) {
-    if ( ReportSent )
+      if (report) {
-    {
+        ReportSent = 0;
-      ReportSent = 0;
+        DCD_EP_Tx (pdev, HID_IN_EP, report, len);
-      DCD_EP_Tx (pdev, HID_IN_EP, report, len);
+      }
+      return USBD_OK;
     }
   }
-  return USBD_OK;
+  return USBD_FAIL;
 }
 
 /**
@@ -425,6 +424,9 @@ static const uint8_t  *USBD_HID_GetCfgDesc (uint8_t speed, uint16_t *length)
   * @param  pdev: device instance
   * @param  epnum: endpoint index
   * @retval status
+
+    This function is called when buffer has been sent over the USB.
+    The TX buffer is now empty and can be filled with new data.
   */
 static uint8_t  USBD_HID_DataIn (void  *pdev, 
                               uint8_t epnum)

radio/src/targets/horus/board.cpp

@@ -211,51 +211,6 @@ void boardOff()
   pwrOff();
 }
 
-#if defined(USB_JOYSTICK) && !defined(SIMU)
-extern USB_OTG_CORE_HANDLE USB_OTG_dev;
-
-/*
-  Prepare and send new USB data packet
-
-  The format of HID_Buffer is defined by
-  USB endpoint description can be found in
-  file usb_hid_joystick.c, variable HID_JOYSTICK_ReportDesc
-*/
-void usbJoystickUpdate(void)
-{
-  static uint8_t HID_Buffer[HID_IN_PACKET];
-
-  //buttons
-  HID_Buffer[0] = 0;
-  HID_Buffer[1] = 0;
-  HID_Buffer[2] = 0;
-  for (int i = 0; i < 8; ++i) {
-    if ( channelOutputs[i+8] > 0 ) {
-      HID_Buffer[0] |= (1 << i);
-    }
-    if ( channelOutputs[i+16] > 0 ) {
-      HID_Buffer[1] |= (1 << i);
-    }
-    if ( channelOutputs[i+24] > 0 ) {
-      HID_Buffer[2] |= (1 << i);
-    }
-  }
-
-  //analog values
-  //uint8_t * p = HID_Buffer + 1;
-  for (int i = 0; i < 8; ++i) {
-    int16_t value = channelOutputs[i] / 8;
-    if ( value > 127 ) value = 127;
-    else if ( value < -127 ) value = -127;
-    HID_Buffer[i+3] = static_cast<int8_t>(value);
-  }
-
-  USBD_HID_SendReport (&USB_OTG_dev, HID_Buffer, HID_IN_PACKET );
-}
-
-#endif // #defined(USB_JOYSTICK) && !defined(SIMU)
-
-
 uint8_t currentTrainerMode = 0xff;
 
 void checkTrainerSettings()

radio/src/targets/horus/board.h

@@ -448,11 +448,15 @@ void backlightEnable(uint8_t dutyCycle);
 #define isBacklightEnabled()  true
 
 // USB driver
-int usbPlugged(void);
+int usbPlugged();
-void usbInit(void);
+void usbInit();
-void usbStart(void);
+void usbStart();
-void usbStop(void);
+void usbStop();
+uint8_t usbStarted();
 void usbSerialPutc(uint8_t c);
+#if defined(USB_JOYSTICK) && !defined(SIMU)
+void usbJoystickUpdate();
+#endif
 #if defined(PCBX12S)
   #define USB_NAME                     "FrSky Horus"
   #define USB_MANUFACTURER             'F', 'r', 'S', 'k', 'y', ' ', ' ', ' '  /* 8 bytes */
@@ -532,10 +536,6 @@ int bluetoothRead(void * buffer, int len);
 void bluetoothWakeup(void);
 void bluetoothDone(void);
 
-#if defined(USB_JOYSTICK) && !defined(SIMU)
-void usbJoystickUpdate(void);
-#endif
-
 extern uint8_t currentTrainerMode;
 void checkTrainerSettings(void);
 

radio/src/targets/taranis/board.cpp

@@ -81,7 +81,9 @@ void interrupt5ms()
 
   if (++pre_scale >= 2) {
     pre_scale = 0 ;
+    DEBUG_TIMER_START(debugTimerPer10ms);
     per10ms();
+    DEBUG_TIMER_STOP(debugTimerPer10ms);
   }
 
 #if defined(ROTARY_ENCODER_NAVIGATION)
@@ -262,51 +264,6 @@ void boardOff()
   pwrOff();
 }
 
-#if defined(USB_JOYSTICK) && !defined(SIMU)
-extern USB_OTG_CORE_HANDLE USB_OTG_dev;
-
-/*
-  Prepare and send new USB data packet
-
-  The format of HID_Buffer is defined by
-  USB endpoint description can be found in
-  file usb_hid_joystick.c, variable HID_JOYSTICK_ReportDesc
-*/
-void usbJoystickUpdate(void)
-{
-  static uint8_t HID_Buffer[HID_IN_PACKET];
-
-  //buttons
-  HID_Buffer[0] = 0;
-  HID_Buffer[1] = 0;
-  HID_Buffer[2] = 0;
-  for (int i = 0; i < 8; ++i) {
-    if ( channelOutputs[i+8] > 0 ) {
-      HID_Buffer[0] |= (1 << i);
-    }
-    if ( channelOutputs[i+16] > 0 ) {
-      HID_Buffer[1] |= (1 << i);
-    }
-    if ( channelOutputs[i+24] > 0 ) {
-      HID_Buffer[2] |= (1 << i);
-    }
-  }
-
-  //analog values
-  //uint8_t * p = HID_Buffer + 1;
-  for (int i = 0; i < 8; ++i) {
-    int16_t value = channelOutputs[i] / 8;
-    if ( value > 127 ) value = 127;
-    else if ( value < -127 ) value = -127;
-    HID_Buffer[i+3] = static_cast<int8_t>(value);
-  }
-
-  USBD_HID_SendReport (&USB_OTG_dev, HID_Buffer, HID_IN_PACKET );
-}
-
-#endif // #defined(USB_JOYSTICK) && !defined(SIMU)
-
-
 uint8_t currentTrainerMode = 0xff;
 
 void checkTrainerSettings()

radio/src/targets/taranis/board.h

@@ -454,11 +454,15 @@ uint8_t isBacklightEnabled(void);
 #endif
 
 // USB driver
-int usbPlugged(void);
+int usbPlugged();
-void usbInit(void);
+void usbInit();
-void usbStart(void);
+void usbStart();
-void usbStop(void);
+void usbStop();
+uint8_t usbStarted();
 void usbSerialPutc(uint8_t c);
+#if defined(USB_JOYSTICK) && !defined(SIMU)
+  void usbJoystickUpdate();
+#endif
 #define USB_NAME                       "FrSky Taranis"
 #define USB_MANUFACTURER               'F', 'r', 'S', 'k', 'y', ' ', ' ', ' '  /* 8 bytes */
 #define USB_PRODUCT                    'T', 'a', 'r', 'a', 'n', 'i', 's', ' '  /* 8 Bytes */
@@ -612,10 +616,6 @@ void setTopBatteryValue(uint32_t volts);
 
 #define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE)
 
-#if defined(USB_JOYSTICK) && !defined(SIMU)
-void usbJoystickUpdate(void);
-#endif
-
 extern uint8_t currentTrainerMode;
 void checkTrainerSettings(void);
 

radio/src/targets/taranis/bootloader/CMakeLists.txt

@@ -55,7 +55,7 @@ set(BOOTLOADER_SRC
   ../../common/arm/stm32/delays.cpp
   ../../common/arm/stm32/usbd_desc.c
   ../../common/arm/stm32/usb_bsp.c
-  ../../common/arm/stm32/usb_driver.c
+  ../../common/arm/stm32/usb_driver.cpp
   ../pwr_driver.cpp
   init.c
   boot.cpp

radio/src/tasks_arm.cpp

@@ -143,7 +143,11 @@ void mixerTask(void * pdata)
 
     uint32_t now = CoGetOSTime();
     bool run = false;
-    if ((now - lastRunTime) > 10) {     // run at least every 20ms
+#if defined(USB_JOYSTICK) && !defined(SIMU)
+    if ((now - lastRunTime) >= (usbStarted() ? 5 : 10)) {     // run at least every 20ms (every 10ms if USB is active)
+#else
+    if ((now - lastRunTime) >= 10) {     // run at least every 20ms
+#endif
       run = true;
     }
     else if (now == nextMixerTime[0]) {
@@ -171,6 +175,10 @@ void mixerTask(void * pdata)
       CoLeaveMutexSection(mixerMutex);
       DEBUG_TIMER_STOP(debugTimerMixer);
 
+#if defined(USB_JOYSTICK) && !defined(SIMU)
+      usbJoystickUpdate();
+#endif
+
 #if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_MAVLINK)
       DEBUG_TIMER_START(debugTimerTelemetryWakeup);
       telemetryWakeup();
@@ -217,6 +225,7 @@ void menusTask(void * pdata)
 #endif
     uint32_t start = (uint32_t)CoGetOSTime();
     DEBUG_TIMER_START(debugTimerPerMain);
+    DEBUG_TIMER_SAMPLE(debugTimerPerMainPeriod);
     perMain();
     DEBUG_TIMER_STOP(debugTimerPerMain);
     // TODO remove completely massstorage from sky9x firmware