From 9b297ef5a109bc1f84f40a1ab9dfbe2808f836d2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 18 Apr 2017 10:01:54 +1000
Subject: [PATCH] HAL_Linux: fixed nested cork/push

don't generate extra pulse sets if we nest
---
 libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp  | 7 +++++++
 libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp  | 3 +++
 libraries/AP_HAL_Linux/RCOutput_Bebop.cpp   | 3 +++
 libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 7 ++++++-
 libraries/AP_HAL_Linux/RCOutput_PRU.cpp     | 3 +++
 libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp   | 3 +++
 libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp    | 3 +++
 libraries/AP_HAL_Linux/RCOutput_qflight.cpp | 6 ++++--
 8 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp b/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp
index ac0fb904da..fc25f36570 100644
--- a/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp
@@ -132,6 +132,7 @@ void RCOutput_AeroIO::set_freq(uint32_t chmask, uint16_t freq_hz)
     }
 
     if (!_corking) {
+        _corking = true;
         push();
     }
 }
@@ -150,6 +151,7 @@ void RCOutput_AeroIO::enable_ch(uint8_t ch)
         return;
     }
     _pending_duty_write_mask |= (1U << ch);
+    _corking = true;
     push();
 }
 
@@ -160,6 +162,7 @@ void RCOutput_AeroIO::disable_ch(uint8_t ch)
     }
     _duty_buffer[ch] = 0;
     _pending_duty_write_mask |= (1U << ch);
+    _corking = true;
     push();
 }
 
@@ -169,6 +172,7 @@ void RCOutput_AeroIO::write(uint8_t ch, uint16_t period_us)
     _duty_buffer[ch] = period_us;
 
     if (!_corking) {
+        _corking = true;
         push();
     }
 }
@@ -180,6 +184,9 @@ void RCOutput_AeroIO::cork()
 
 void RCOutput_AeroIO::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
 
     for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
index 0c1253a46f..126cc05c61 100644
--- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp
@@ -142,6 +142,9 @@ void RCOutput_AioPRU::cork(void)
 
 void RCOutput_AioPRU::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<PWM_CHAN_COUNT; i++) {
         if (pending_mask & (1U<<i)) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
index 462bb55dc2..4f1a3cea80 100644
--- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
@@ -418,6 +418,9 @@ void RCOutput_Bebop::cork()
 
 void RCOutput_Bebop::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
     pthread_mutex_lock(&_mutex);
     memcpy(_period_us, _request_period_us, sizeof(_period_us));
diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
index 602e0893d4..862d069f02 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
@@ -176,8 +176,10 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
     _pulses_buffer[ch] = period_us;
     _pending_write_mask |= (1U << ch);
 
-    if (!_corking)
+    if (!_corking) {
+        _corking = true;
         push();
+    }
 }
 
 void RCOutput_PCA9685::cork()
@@ -187,6 +189,9 @@ void RCOutput_PCA9685::cork()
 
 void RCOutput_PCA9685::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
 
     if (_pending_write_mask == 0)
diff --git a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp
index 6e5f9c6b38..f82f33d53d 100644
--- a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp
@@ -99,6 +99,9 @@ void RCOutput_PRU::cork(void)
 
 void RCOutput_PRU::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<ARRAY_SIZE(pending); i++) {
         if (pending_mask & (1U << i)) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp
index d0a4924e44..b33afc54c1 100644
--- a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp
@@ -136,6 +136,9 @@ void RCOutput_Sysfs::cork(void)
 
 void RCOutput_Sysfs::push(void)
 {
+    if (!_corked) {
+        return;
+    }
     for (uint8_t i=0; i<_channel_count; i++) {
         if ((1U<<i) & _pending_mask) {
             _pwm_channels[i]->set_duty_cycle(usec_to_nsec(_pending[i]));
diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp
index f37a761df6..a9d1d20b05 100644
--- a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp
@@ -119,6 +119,9 @@ void RCOutput_ZYNQ::cork(void)
 
 void RCOutput_ZYNQ::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<MAX_ZYNQ_PWMS; i++) {
         if (pending_mask & (1U << i)) {
diff --git a/libraries/AP_HAL_Linux/RCOutput_qflight.cpp b/libraries/AP_HAL_Linux/RCOutput_qflight.cpp
index 5a9a608dc1..c80113f47d 100644
--- a/libraries/AP_HAL_Linux/RCOutput_qflight.cpp
+++ b/libraries/AP_HAL_Linux/RCOutput_qflight.cpp
@@ -180,8 +180,10 @@ void RCOutput_QFLIGHT::cork(void)
 
 void RCOutput_QFLIGHT::push(void)
 {
-    corked = false;
-    need_write = true;
+    if (corked) {
+        corked = false;
+        need_write = true;
+    }
 }
 
 #endif // CONFIG_HAL_BOARD_SUBTYPE