From 0fcc6d7389809fe63c5755f28cc40e860e0dd706 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 4 Jan 2013 22:25:36 +1100
Subject: [PATCH] HAL_PX4: added an RC Output driver

---
 libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h |  1 +
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp      |  5 +-
 libraries/AP_HAL_PX4/RCOutput.cpp           | 86 +++++++++++++++++++++
 libraries/AP_HAL_PX4/RCOutput.h             | 26 +++++++
 4 files changed, 117 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_HAL_PX4/RCOutput.cpp
 create mode 100644 libraries/AP_HAL_PX4/RCOutput.h

diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
index d78e54ce57..2a71997214 100644
--- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
+++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
@@ -8,6 +8,7 @@ namespace PX4 {
 	class PX4UARTDriver;
 	class PX4EEPROMStorage;
 	class PX4RCInput;
+	class PX4RCOutput;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__
diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index b2ba7b5bbf..2401a377fc 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -12,6 +12,7 @@
 #include "UARTDriver.h"
 #include "Storage.h"
 #include "RCInput.h"
+#include "RCOutput.h"
 
 #include <AP_HAL_Empty.h>
 #include <AP_HAL_Empty_Private.h>
@@ -29,13 +30,13 @@ static Empty::EmptyI2CDriver  i2cDriver;
 static Empty::EmptySPIDeviceManager spiDeviceManager;
 static Empty::EmptyAnalogIn analogIn;
 static Empty::EmptyGPIO gpioDriver;
-static Empty::EmptyRCOutput rcoutDriver;
 static Empty::EmptyUtil utilInstance;
 
 static PX4ConsoleDriver consoleDriver;
 static PX4Scheduler schedulerInstance;
 static PX4EEPROMStorage storageDriver;
 static PX4RCInput rcinDriver;
+static PX4RCOutput rcoutDriver;
 
 #define UARTA_DEVICE "/dev/ttyS2"
 #define UARTB_DEVICE "/dev/ttyS3"
@@ -72,10 +73,12 @@ static int main_loop(int argc, char **argv)
 {
     extern void setup(void);
     extern void loop(void);
+
     hal.uartA->begin(115200);
     hal.console->init((void*) hal.uartA);
     hal.scheduler->init(NULL);
     hal.rcin->init(NULL);
+    hal.rcout->init(NULL);
 
     setup();
 
diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
new file mode 100644
index 0000000000..d4671d9cf6
--- /dev/null
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -0,0 +1,86 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include "RCOutput.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_pwm_output.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace PX4;
+
+void PX4RCOutput::init(void* unused) 
+{
+    _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+    if (_pwm_fd == -1) {
+        hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
+    }
+    ioctl(_pwm_fd, PWM_SERVO_ARM, 0);
+}
+
+void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) 
+{
+	// no support for this yet
+}
+
+uint16_t PX4RCOutput::get_freq(uint8_t ch) 
+{
+	return 50;
+}
+
+void PX4RCOutput::enable_ch(uint8_t ch)
+{
+    // channels are always enabled ...
+}
+
+void PX4RCOutput::enable_mask(uint32_t chmask)
+{
+    // channels are always enabled ...
+}
+
+void PX4RCOutput::disable_ch(uint8_t ch)
+{
+    // channels are always enabled ...
+}
+
+void PX4RCOutput::disable_mask(uint32_t chmask)
+{
+    // channels are always enabled ...
+}
+
+void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
+{
+    ioctl(_pwm_fd, PWM_SERVO_SET(ch), (unsigned long)period_us);
+}
+
+void PX4RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+    for (uint8_t i=0; i<len; i++) {
+        write(i, period_us[i]);
+    }
+}
+
+uint16_t PX4RCOutput::read(uint8_t ch) 
+{
+    servo_position_t pos;
+	if (ioctl(_pwm_fd, PWM_SERVO_GET(ch), (unsigned long)&pos) == 0) {
+       return pos;
+    }
+    return 0;
+}
+
+void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
+{
+    for (uint8_t i=0; i<len; i++) {
+        period_us[i] = read(i);
+    }
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
new file mode 100644
index 0000000000..6c00e82850
--- /dev/null
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -0,0 +1,26 @@
+
+#ifndef __AP_HAL_PX4_RCOUTPUT_H__
+#define __AP_HAL_PX4_RCOUTPUT_H__
+
+#include <AP_HAL_PX4.h>
+
+class PX4::PX4RCOutput : public AP_HAL::RCOutput 
+{
+public:
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     enable_mask(uint32_t chmask);
+    void     disable_ch(uint8_t ch);
+    void     disable_mask(uint32_t chmask);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+
+private:
+    int _pwm_fd;
+};
+
+#endif // __AP_HAL_PX4_RCOUTPUT_H__