From e5549c90a14ebf99fcee832f6e7976c08eefad8a Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Wed, 27 Aug 2014 11:52:07 +0200
Subject: [PATCH] HAL_PX4: do not overwrite disabled channels with zeros

Fixes #1321
---
 libraries/AP_HAL_PX4/RCOutput.cpp | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index b17a21e62f..3b295b97a8 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -49,6 +49,12 @@ void PX4RCOutput::init(void* unused)
         hal.console->printf("RCOutput: failed to open /dev/px4fmu");
         return;
     }
+
+    // ensure not to write zeros to disabled channels
+    _enabled_channels = 0;
+    for (int i=0; i < PX4_NUM_OUTPUT_CHANNELS; i++) {
+        _period[i] = PWM_IGNORE_THIS_CHANNEL;
+    }
 }
 
 
@@ -133,18 +139,27 @@ uint16_t PX4RCOutput::get_freq(uint8_t ch)
 
 void PX4RCOutput::enable_ch(uint8_t ch)
 {
-    if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
+    if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
+        return;
+    }
+    else if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
         // this is the first enable of an auxillary channel - setup
         // aux channels now. This delayed setup makes it possible to
         // use BRD_PWM_COUNT to setup the number of PWM channels.
         _init_alt_channels();
     }
     _enabled_channels |= (1U<<ch);
+    _period[ch] = 0;
 }
 
 void PX4RCOutput::disable_ch(uint8_t ch)
 {
+    if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
+        return;
+    }
+
     _enabled_channels &= ~(1U<<ch);
+    _period[ch] = PWM_IGNORE_THIS_CHANNEL;
 }
 
 void PX4RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)