From 759b0d6629800df17ed86e50486270444d546e5a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 13 Apr 2016 17:46:16 +1000
Subject: [PATCH] HAL_PX4: only support oneshot on FMU outputs for now

oneshot on PX4IO gives some very weird results. I think it is doable,
but will take a bit more work
---
 libraries/AP_HAL_PX4/RCOutput.cpp | 27 +++++++++++++++++++++++++--
 libraries/AP_HAL_PX4/RCOutput.h   |  2 ++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index 1a2e1541f2..e40fd027b0 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -462,8 +462,7 @@ void PX4RCOutput::push()
         _send_outputs();
         if (_fast_channel_mask != 0) {
             // this is a crude way of triggering immediate output
-            set_freq(_fast_channel_mask, 400);
-            set_freq(_fast_channel_mask, 0);
+            _trigger_fast_output();
         }
     }
 }
@@ -475,4 +474,28 @@ void PX4RCOutput::_timer_tick(void)
     }
 }
 
+/*
+  trigger immediate output on fast channels. This only works on FMU,
+  not IO. It takes advantage of the way the rate update works on FMU
+  to trigger a oneshot output
+ */
+void PX4RCOutput::_trigger_fast_output(void)
+{
+    uint32_t primary_mask = _fast_channel_mask & ((1UL<<_servo_count)-1);
+    uint32_t alt_mask = _fast_channel_mask >> _servo_count;    
+    if (_alt_fd == -1) {
+        // we're on a FMU only board
+        if (primary_mask) {
+            set_freq_fd(_pwm_fd, primary_mask, 400);
+            set_freq_fd(_pwm_fd, primary_mask, 0);
+        }
+    } else {
+        // we're on a board with px4io
+        if (alt_mask) {
+            set_freq_fd(_alt_fd, alt_mask, 400);
+            set_freq_fd(_alt_fd, alt_mask, 0);
+        }
+    }
+}
+
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index 7a1ef8a851..65658ba827 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -70,4 +70,6 @@ private:
     bool _corking;
     enum output_mode _output_mode = MODE_PWM_NORMAL;
     void _send_outputs(void);
+
+    void _trigger_fast_output();
 };