From 7c23e3377574e7d9f324ec89b8b73efa30e4e523 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 17 Jan 2018 21:25:02 +1100
Subject: [PATCH] HAL_Chibios: set min output rate for oneshot to 100Hz

this will keep ESCs happy during gyro cal
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp  | 83 +++++++++++++++++++-------
 libraries/AP_HAL_ChibiOS/RCOutput.h    | 17 ++++++
 libraries/AP_HAL_ChibiOS/Scheduler.cpp |  2 +-
 3 files changed, 79 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index d720e729fb..92c1551b02 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/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) {
-        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 (need_trigger & (1U<<i)) {
-                // 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;
+    if (widest_pulse > 2300) {
+        widest_pulse = 2300;
+    }
+    trigger_widest_pulse = widest_pulse;
+    
+    trigger_groups = need_trigger;
+    
+    if (trigger_groups && _output_mode == MODE_PWM_ONESHOT) {
+        trigger_oneshot();
     }
 }
 
@@ -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();
+    }
+}
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 04f50ae598..d7045605ab 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -57,6 +57,11 @@ public:
       set default update rate
      */
     void set_default_rate(uint16_t rate_hz) override;
+
+    /*
+      timer push (for oneshot min rate)
+     */
+    void timer_tick(void) override;
     
 private:
     struct pwm_group {
@@ -91,7 +96,19 @@ 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);
 };
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 99a1a6b030..7c7f8e4a2d 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/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();