From 8f2f4b1bc5c5cf1a5c6d44c17863c3f685c2db77 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 25 Jan 2013 19:16:28 +1100
Subject: [PATCH] HAL_PX4: use write() to /dev/pwm_output for servo output

this lowest the cost of PWM output a lot, but relies on the new I2C
based IO firmware
---
 libraries/AP_HAL_PX4/RCOutput.cpp  | 25 ++++++++++++++++++++++++-
 libraries/AP_HAL_PX4/RCOutput.h    |  7 +++++++
 libraries/AP_HAL_PX4/Scheduler.cpp |  4 ++++
 3 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index b47d32cab6..e49baff3d6 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -18,6 +18,7 @@ using namespace PX4;
 
 void PX4RCOutput::init(void* unused) 
 {
+    _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
     _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
     if (_pwm_fd == -1) {
         hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
@@ -67,9 +68,12 @@ void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
     if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
         return;
     }
+    if (ch > _max_channel) {
+        _max_channel = ch;
+    }
     if (period_us != _period[ch]) {
-        ioctl(_pwm_fd, PWM_SERVO_SET(ch), (unsigned long)period_us);
         _period[ch] = period_us;
+        _need_update = true;
     }
 }
 
@@ -95,4 +99,23 @@ void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
     }
 }
 
+void PX4RCOutput::_timer_tick(void)
+{
+    uint32_t now = hal.scheduler->micros();
+
+    // always send at least at 20Hz, otherwise the IO board may think
+    // we are dead
+    if (now - _last_output > 50000) {
+        _need_update = true;
+    }
+
+    if (_need_update && _pwm_fd != -1) {
+        _need_update = false;
+        perf_begin(_perf_rcout);
+        ::write(_pwm_fd, _period, _max_channel*sizeof(_period[0]));
+        perf_end(_perf_rcout);
+        _last_output = now;
+    }
+}
+
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index 9754900f6f..78850aafd5 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -3,6 +3,7 @@
 #define __AP_HAL_PX4_RCOUTPUT_H__
 
 #include <AP_HAL_PX4.h>
+#include <systemlib/perf_counter.h>
 
 #define PX4_NUM_OUTPUT_CHANNELS 16
 
@@ -21,10 +22,16 @@ public:
     uint16_t read(uint8_t ch);
     void     read(uint16_t* period_us, uint8_t len);
 
+    void _timer_tick(void);
+
 private:
     int _pwm_fd;
     uint16_t _freq_hz;
     uint16_t _period[PX4_NUM_OUTPUT_CHANNELS];
+    volatile uint8_t _max_channel;
+    volatile bool _need_update;
+    perf_counter_t  _perf_rcout;
+    uint32_t _last_output;
 };
 
 #endif // __AP_HAL_PX4_RCOUTPUT_H__
diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 797fbf4a03..1e2ac97989 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -18,6 +18,7 @@
 
 #include "UARTDriver.h"
 #include "Storage.h"
+#include "RCOutput.h"
 
 using namespace PX4;
 
@@ -190,6 +191,9 @@ void *PX4Scheduler::_timer_thread(void)
         perf_begin(_perf_timers);
         _run_timers(true);
         perf_end(_perf_timers);
+
+        // process any pending RC output requests
+        ((PX4RCOutput *)hal.rcout)->_timer_tick();
     }
     return NULL;
 }