Added logic for auto sampling mode change based on sampling frequency

Author: richardclli

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

@@ -36,7 +36,10 @@
 
 #include <string.h>
 
-#define SAMPLING_TIMEOUT_US 500
+#define SAMPLING_TIMEOUT_US             1000  // us
+#define RESAMPLING_THRESHOLD            2500  // us, 400Hz freq = 2500us period
+#define RESAMPLING_SWITCHING_THRESHOLD   200  // us
+#define MODE_CHANGE_DELAY                100  // ms
 
 static const stm32_usart_t fsUSART = {
   .USARTx = FLYSKY_HALL_SERIAL_USART,
@@ -68,13 +71,14 @@ static uint8_t _fs_hall_command[8] __DMA;
 static void* _fs_usart_ctx = nullptr;
 
 static volatile bool _fs_gimbal_detected;
-static uint8_t _fs_gimbal_version = GIMBAL_V1;
-static uint8_t _fs_gimbal_mode = V1_MODE;
-static uint8_t _fs_gimbal_mode_cmd = V1_MODE;
-static bool _fs_gimbal_read_finished = true;
-static uint32_t _fs_lastReadTick = 0;
-static uint32_t _fs_readTick;
-static uint32_t _fs_sync_period = 0;
+static uint8_t _fs_gimbal_version;
+static uint8_t _fs_gimbal_mode;
+static uint8_t _fs_gimbal_mode_change;
+static uint32_t _fs_gimbal_last_mode_change_tick;
+static bool _fs_gimbal_cmd_finished;
+static uint32_t _fs_gimbal_lastReadTick;
+static uint32_t _fs_gimbal_readTick;
+static volatile uint32_t _fs_gimbal_sync_period;
 
 static int _fs_get_byte(uint8_t* data)
 {
@@ -146,6 +150,11 @@ static void _fs_parse(STRUCT_HALL *hallBuffer, unsigned char ch)
 
 void _fs_send_cmd(uint8_t id, uint8_t payload)
 {
+  if (!_fs_gimbal_cmd_finished) {
+    // Skip command when last command not finished
+    return;
+  }
+
   _fs_hall_command[0] = FLYSKY_HALL_PROTOLO_HEAD;
   _fs_hall_command[1] = id;
   _fs_hall_command[2] = 0x01;
@@ -156,7 +165,9 @@ void _fs_send_cmd(uint8_t id, uint8_t payload)
   _fs_hall_command[4] = crc & 0xff;
   _fs_hall_command[5] = crc >>8 & 0xff ;
 
+  _fs_gimbal_cmd_finished = false;
   STM32SerialDriver.sendBuffer(_fs_usart_ctx, _fs_hall_command, 6);
+//  TRACE("Flysky Gimbal: Sent command, id = %d, payload = %d", id, payload);
 }
 
 void _fs_cmd_get_version()
@@ -166,12 +177,7 @@ void _fs_cmd_get_version()
 
 void _fs_cmd_set_mode(V2_GIMBAL_MODE mode)
 {
-  if (_fs_gimbal_mode != _fs_gimbal_mode_cmd) {
-    // Last command not responsed yet
-    return;
-  }
-
-  _fs_gimbal_mode_cmd = mode;
+  _fs_gimbal_mode_change = mode;
   _fs_send_cmd(0x41, mode);
 }
 
@@ -202,12 +208,13 @@ static void flysky_gimbal_loop(void*)
         case TRANSFER_DIR_RFMODULE:
           int16_t* p_values = (int16_t*)HallProtocol.data;
           if (HallProtocol.hallID.hall_Id.packetID == FLYSKY_PACKET_CHANNEL_ID) {
+            _fs_gimbal_cmd_finished = true;
             uint16_t* adcValues = getAnalogValues();
             for (uint8_t i = 0; i < 4; i++) {
               adcValues[i] = FLYSKY_OFFSET_VALUE - p_values[i];
             }
-            _fs_gimbal_read_finished = true;
           } else if (HallProtocol.hallID.hall_Id.packetID == FLYSKY_PACKET_VERSION_ID) {
+            _fs_gimbal_cmd_finished = true;
             uint16_t minorVersion = p_values[6];
             uint16_t majorVersion = p_values[7];
             if (majorVersion == 2 && minorVersion >= 1) {
@@ -216,7 +223,9 @@ static void flysky_gimbal_loop(void*)
               _fs_cmd_set_mode(SYNC_RESAMPLING);
             }
           } else if (HallProtocol.hallID.hall_Id.packetID == FLYSKY_PACKET_MODE_ID) {
-            _fs_gimbal_mode = _fs_gimbal_mode_cmd;
+            _fs_gimbal_cmd_finished = true;
+            _fs_gimbal_mode = _fs_gimbal_mode_change;
+            TRACE("Flysky Gimbal: Mode changed successfully, mode = %d", _fs_gimbal_mode);
           }
           break;
       }
@@ -242,7 +251,15 @@ bool flysky_gimbal_init(bool force)
     .polarity = ETX_Pol_Normal,
   };
 
+  // Init variables
   _fs_gimbal_detected = force;
+  _fs_gimbal_version = GIMBAL_V1;
+  _fs_gimbal_mode = V1_MODE;
+  _fs_gimbal_last_mode_change_tick = 0;
+  _fs_gimbal_cmd_finished = true;
+  _fs_gimbal_lastReadTick = 0;
+  _fs_gimbal_sync_period = 0;
+  
   _fs_usart_ctx = STM32SerialDriver.init(REF_STM32_SERIAL_PORT(FSGimbal), &cfg);
   STM32SerialDriver.setIdleCb(_fs_usart_ctx, flysky_gimbal_loop, 0);
 
@@ -266,26 +283,33 @@ bool flysky_gimbal_init(bool force)
 void flysky_gimbal_start_read()
 {
   if(_fs_sync_enabled()) {
-    if (_fs_gimbal_read_finished) {
-      _fs_gimbal_read_finished = false;
-      _fs_lastReadTick = _fs_readTick;
-      _fs_readTick = timersGetUsTick();
-      if (_fs_lastReadTick != 0) {
-        _fs_sync_period = _fs_readTick - _fs_lastReadTick;
+    _fs_gimbal_lastReadTick = _fs_gimbal_readTick;
+    _fs_gimbal_readTick = timersGetUsTick();
+    if (_fs_gimbal_lastReadTick != 0) {
+      _fs_gimbal_sync_period = _fs_gimbal_readTick - _fs_gimbal_lastReadTick;
+      uint32_t tick = timersGetMsTick();
+      if (tick - _fs_gimbal_last_mode_change_tick >= MODE_CHANGE_DELAY) {    
+        // Prevent mode change too often
+        _fs_gimbal_last_mode_change_tick = tick;
+        if (_fs_gimbal_mode == SYNC_SAMPLING && _fs_gimbal_sync_period < RESAMPLING_THRESHOLD) {
+          _fs_cmd_set_mode(SYNC_RESAMPLING);
+        } else if(_fs_gimbal_mode == SYNC_RESAMPLING && _fs_gimbal_sync_period >= RESAMPLING_THRESHOLD + RESAMPLING_SWITCHING_THRESHOLD) {
+          _fs_cmd_set_mode(SYNC_SAMPLING);
+        }
       }
-      _fs_cmd_start_read();
-    }    
+    }
+    _fs_cmd_start_read();
   }
 }
 
 void flysky_gimbal_wait_completion()
 {
   if(_fs_sync_enabled()) {
     auto timeout = timersGetUsTick();
-    while(!_fs_gimbal_read_finished) {
+    while(!_fs_gimbal_cmd_finished) {
       // busy wait
       if ((uint32_t)(timersGetUsTick() - timeout) >= SAMPLING_TIMEOUT_US) {
-        TRACE("Gimbal timeout");
+//        TRACE("Gimbal timeout");
         return;
       }
     }

radio/src/tasks/mixer_task.cpp

@@ -255,10 +255,10 @@ void doMixerCalculations()
   DEBUG_TIMER_STOP(debugTimerGetAdc);
 
  // Need to put all in a group to ensure DMA transfer will not affect ADC sampling
-#if defined(FLYSKY_GIMBAL)
+#if defined(FLYSKY_GIMBAL) && !defined(SIMU)
   flysky_gimbal_start_read();
 #endif
-#if defined(FLYSKY_GIMBAL)
+#if defined(FLYSKY_GIMBAL) && !defined(SIMU)
   flysky_gimbal_wait_completion();
 #endif