HAL_Chibios: set min output rate for oneshot to 100Hz

this will keep ESCs happy during gyro cal

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -52,6 +52,7 @@ void RCOutput::init()
         total_channels += chan_offset;
     }
 #endif
+    chMtxObjectInit(&trigger_mutex);
 }
 
 void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
@@ -280,28 +281,15 @@ void RCOutput::push_local(void)
         }
     }
 
-    if (need_trigger && _output_mode == MODE_PWM_ONESHOT) {
+    if (widest_pulse > 2300) {
-        uint64_t now = AP_HAL::micros64();
+        widest_pulse = 2300;
-        if (now < min_pulse_trigger_us) {
+    }
-            // guarantee minimum pulse separation
+    trigger_widest_pulse = widest_pulse;
-            hal.scheduler->delay_microseconds(min_pulse_trigger_us - now);
+    
-        }
+    trigger_groups = need_trigger;
-        osalSysLock();
+    
-        for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
+    if (trigger_groups && _output_mode == MODE_PWM_ONESHOT) {
-            if (need_trigger & (1U<<i)) {
+        trigger_oneshot();
-                // this triggers pulse output for a channel group
-                pwm_group_list[i].pwm_drv->tim->EGR = STM32_TIM_EGR_UG;
-            }
-        }
-        osalSysUnlock();
-        if (widest_pulse > 2300) {
-            widest_pulse = 2300;
-        }
-        /*
-          calculate time that we are allowed to trigger next pulse
-          to guarantee at least a 50us gap between pulses
-        */
-        min_pulse_trigger_us = AP_HAL::micros64() + widest_pulse + 50;
     }
 }
 
@@ -355,6 +343,7 @@ void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
         period_us[i] = read_last_sent(i);
     }
 }
+
 /*
   setup output mode
  */
@@ -444,3 +433,53 @@ bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
 #endif
     return false;
 }
+
+/*
+  trigger output groups for oneshot mode
+ */
+void RCOutput::trigger_oneshot(void)
+{
+    if (!chMtxTryLock(&trigger_mutex)) {
+        return;
+    }
+    uint64_t now = AP_HAL::micros64();
+    if (now < min_pulse_trigger_us) {
+        // guarantee minimum pulse separation
+        hal.scheduler->delay_microseconds(min_pulse_trigger_us - now);
+    }
+    osalSysLock();
+    for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
+        if (trigger_groups & (1U<<i)) {
+            // this triggers pulse output for a channel group
+            pwm_group_list[i].pwm_drv->tim->EGR = STM32_TIM_EGR_UG;
+        }
+    }
+    osalSysUnlock();
+    /*
+      calculate time that we are allowed to trigger next pulse
+      to guarantee at least a 50us gap between pulses
+    */
+    min_pulse_trigger_us = AP_HAL::micros64() + trigger_widest_pulse + 50;
+    chMtxUnlock(&trigger_mutex);
+}
+
+/*
+  periodic timer. The only need for a periodic timer is in oneshot
+  mode where we want to sustain a minimum output rate for when the
+  main loop is busy doing something like gyro calibration
+
+  A mininum output rate helps with some oneshot ESCs
+ */
+void RCOutput::timer_tick(void)
+{
+    if (_output_mode != MODE_PWM_ONESHOT ||
+        trigger_groups == 0 ||
+        min_pulse_trigger_us == 0) {
+        return;
+    }
+    uint64_t now = AP_HAL::micros64();
+    if (now - min_pulse_trigger_us > 10000) {
+        // trigger at a minimum of 100Hz
+        trigger_oneshot();
+    }
+}

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -58,6 +58,11 @@ public:
      */
     void set_default_rate(uint16_t rate_hz) override;
 
+    /*
+      timer push (for oneshot min rate)
+     */
+    void timer_tick(void) override;
+    
 private:
     struct pwm_group {
         // only advanced timers can do high clocks needed for more than 400Hz
@@ -92,6 +97,18 @@ private:
     // min time to trigger next pulse to prevent overlap
     uint64_t min_pulse_trigger_us;
 
+    // mutex for oneshot triggering
+    mutex_t trigger_mutex;
+
+    // which output groups need triggering
+    uint8_t trigger_groups;
+
+    // widest pulse for oneshot triggering
+    uint16_t trigger_widest_pulse;
+    
     // push out values to local PWM
     void push_local(void);
+
+    // trigger oneshot pulses
+    void trigger_oneshot(void);
 };

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -253,7 +253,7 @@ void Scheduler::_timer_thread(void *arg)
         sched->_run_timers(true);
 
         // process any pending RC output requests
-        //hal.rcout->timer_tick();
+        hal.rcout->timer_tick();
 
         // process any pending RC input requests
         ((RCInput *)hal.rcin)->_timer_tick();