AP_HAL_Linux: RCOutput: Add LinuxRCOutput_Sysfs

This RCOutput works over Linux PWM sysfs interface.

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -34,6 +34,7 @@ namespace Linux {
     class RCOutput_Raspilot;
     class RCOutput_ZYNQ;
     class RCOutput_Bebop;
+    class RCOutput_Sysfs;
     class Semaphore;
     class Scheduler;
     class Util;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -28,6 +28,7 @@
 #include "RCOutput_ZYNQ.h"
 #include "RCOutput_Bebop.h"
 #include "RCOutput_Raspilot.h"
+#include "RCOutput_Sysfs.h"
 #include "Semaphores.h"
 #include "Scheduler.h"
 #include "ToneAlarmDriver.h"

libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp

@@ -0,0 +1,126 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2015  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include "RCOutput_Sysfs.h"
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+
+namespace Linux {
+
+static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+RCOutput_Sysfs::RCOutput_Sysfs(uint8_t chip, uint8_t channel_count)
+    : _chip(chip)
+    , _channel_count(channel_count)
+    , _pwm_channels(new PWM_Sysfs *[_channel_count])
+{
+}
+
+RCOutput_Sysfs::~RCOutput_Sysfs()
+{
+    for (uint8_t i = 0; i < _channel_count; i++) {
+        delete _pwm_channels[i];
+    }
+
+    delete _pwm_channels;
+}
+
+void RCOutput_Sysfs::init(void *machtnichts)
+{
+    for (uint8_t i = 0; i < _channel_count; i++) {
+        _pwm_channels[i] = new PWM_Sysfs(_chip, i);
+        if (!_pwm_channels[i]) {
+            hal.scheduler->panic("RCOutput_Sysfs_PWM: Unable to setup PWM pin.");
+        }
+        _pwm_channels[i]->enable(false);
+
+        /* Set the initial frequency */
+        _pwm_channels[i]->set_freq(50);
+        _pwm_channels[i]->set_duty_cycle(0);
+        _pwm_channels[i]->set_polarity(PWM_Sysfs::Polarity::NORMAL);
+    }
+}
+
+void RCOutput_Sysfs::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    for (uint8_t i = 0; i < _channel_count; i++) {
+        if (chmask & 1 << i) {
+            _pwm_channels[i]->set_freq(freq_hz);
+        }
+    }
+}
+
+uint16_t RCOutput_Sysfs::get_freq(uint8_t ch)
+{
+    if (ch >= _channel_count) {
+        return 0;
+    }
+
+    return _pwm_channels[ch]->get_freq();
+}
+
+void RCOutput_Sysfs::enable_ch(uint8_t ch)
+{
+    if (ch >= _channel_count) {
+        return;
+    }
+
+    _pwm_channels[ch]->enable(true);
+}
+
+void RCOutput_Sysfs::disable_ch(uint8_t ch)
+{
+    if (ch >= _channel_count) {
+        return;
+    }
+
+    _pwm_channels[ch]->enable(false);
+}
+
+void RCOutput_Sysfs::write(uint8_t ch, uint16_t period_us)
+{
+    if (ch >= _channel_count) {
+        return;
+    }
+
+    _pwm_channels[ch]->set_duty_cycle(usec_to_nsec(period_us));
+}
+
+uint16_t RCOutput_Sysfs::read(uint8_t ch)
+{
+    if (ch >= _channel_count) {
+        return 0;
+    }
+
+    return nsec_to_usec(_pwm_channels[ch]->get_duty_cycle());
+}
+
+void RCOutput_Sysfs::read(uint16_t *period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++) {
+        period_us[i] = read(i);
+    }
+}
+}
+
+#endif

libraries/AP_HAL_Linux/RCOutput_Sysfs.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "AP_HAL_Linux.h"
+#include "PWM_Sysfs.h"
+
+class Linux::RCOutput_Sysfs : public AP_HAL::RCOutput {
+public:
+    RCOutput_Sysfs(uint8_t chip, uint8_t channel_count);
+    ~RCOutput_Sysfs();
+
+    static RCOutput_Sysfs *from(AP_HAL::RCOutput *rcoutput)
+    {
+        return static_cast<RCOutput_Sysfs *>(rcoutput);
+    }
+
+    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 disable_ch(uint8_t ch);
+    void write(uint8_t ch, uint16_t period_us);
+    uint16_t read(uint8_t ch);
+    void read(uint16_t *period_us, uint8_t len);
+
+private:
+    const uint8_t _chip;
+    const uint8_t _channel_count;
+    PWM_Sysfs **_pwm_channels;
+};