HAL_Linux: added RCOutput_Disco

combines Sysfs and Bebop RCOutput code

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -42,6 +42,7 @@ namespace Linux {
     class RCOutput_Bebop;
     class RCOutput_Sysfs;
     class RCOutput_QFLIGHT;
+    class RCOutput_Disco;
     class Semaphore;
     class Scheduler;
     class Util;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -35,6 +35,7 @@
 #include "RCOutput_Raspilot.h"
 #include "RCOutput_Sysfs.h"
 #include "RCOutput_qflight.h"
+#include "RCOutput_Disco.h"
 #include "Semaphores.h"
 #include "Scheduler.h"
 #include "ToneAlarm.h"

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -180,7 +180,7 @@ static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(i2c_devpaths, PC
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
 static RCOutput_QFLIGHT rcoutDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
-static RCOutput_Sysfs rcoutDriver(0, 3, 2);
+static RCOutput_Disco rcoutDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
 static RCOutput_Sysfs rcoutDriver(0, 0, 14);
 #else

libraries/AP_HAL_Linux/RCOutput_Disco.cpp

@@ -0,0 +1,105 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2016  Andrew Tridgell. 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/>.
+ */
+
+/*
+  RCOutput on the Disco combines I2C motor output (for channel 3) with
+  PWM output for the other channels. This class is a wrapper around
+  the two other classes
+ */
+#include "RCOutput_Disco.h"
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+
+namespace Linux {
+
+void RCOutput_Disco::init()
+{
+    sysfs_out.init();
+    bebop_out.init();
+}
+
+void RCOutput_Disco::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    for (uint8_t i = 0; i < ARRAY_SIZE(output_table); i++) {
+        if (chmask & (1U << i)) {
+            output_table[i].output.set_freq(1U<<output_table[i].channel, freq_hz);
+        }
+    }
+}
+
+uint16_t RCOutput_Disco::get_freq(uint8_t ch)
+{
+    if (ch >= ARRAY_SIZE(output_table)) {
+        return 0;
+    }
+    return output_table[ch].output.get_freq(output_table[ch].channel);
+}
+
+void RCOutput_Disco::enable_ch(uint8_t ch)
+{
+    if (ch >= ARRAY_SIZE(output_table)) {
+        return;
+    }
+    output_table[ch].output.enable_ch(output_table[ch].channel);
+}
+
+void RCOutput_Disco::disable_ch(uint8_t ch)
+{
+    if (ch >= ARRAY_SIZE(output_table)) {
+        return;
+    }
+    output_table[ch].output.disable_ch(output_table[ch].channel);
+}
+
+void RCOutput_Disco::write(uint8_t ch, uint16_t period_us)
+{
+    if (ch >= ARRAY_SIZE(output_table)) {
+        return;
+    }
+    output_table[ch].output.write(output_table[ch].channel, period_us);
+}
+
+uint16_t RCOutput_Disco::read(uint8_t ch)
+{
+    if (ch >= ARRAY_SIZE(output_table)) {
+        return 0;
+    }
+    return output_table[ch].output.read(output_table[ch].channel);
+}
+
+void RCOutput_Disco::read(uint16_t *period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++) {
+        period_us[i] = read(i);
+    }
+}
+
+void RCOutput_Disco::cork(void)
+{
+    sysfs_out.cork();
+    bebop_out.cork();
+}
+
+void RCOutput_Disco::push(void)
+{
+    sysfs_out.push();
+    bebop_out.push();
+}
+    
+}

libraries/AP_HAL_Linux/RCOutput_Disco.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "AP_HAL_Linux.h"
+#include "RCOutput_Sysfs.h"
+#include "RCOutput_Bebop.h"
+
+class Linux::RCOutput_Disco : public AP_HAL::RCOutput {
+public:
+    RCOutput_Disco(void) {}
+    ~RCOutput_Disco() {}
+
+    static RCOutput_Disco *from(AP_HAL::RCOutput *rcoutput)
+    {
+        return static_cast<RCOutput_Disco *>(rcoutput);
+    }
+
+    void init() override;
+    void set_freq(uint32_t chmask, uint16_t freq_hz) override;
+    uint16_t get_freq(uint8_t ch) override;
+    void enable_ch(uint8_t ch) override;
+    void disable_ch(uint8_t ch) override;
+    void write(uint8_t ch, uint16_t period_us) override;
+    uint16_t read(uint8_t ch) override;
+    void read(uint16_t *period_us, uint8_t len) override;
+    void cork() override;
+    void push() override;
+
+private:
+    // Disco RC output combines methods from Sysfs and Bebop
+    RCOutput_Sysfs sysfs_out{0, 1, 6};
+    RCOutput_Bebop bebop_out;
+
+    /*
+      use a table to provide the mapping to channel numbers in each
+      backend
+     */
+    typedef struct {
+        RCOutput &output;
+        uint8_t channel;
+    } output_table_t;
+    const output_table_t output_table[7] = {
+        { sysfs_out, 3 },
+        { sysfs_out, 2 },
+        { bebop_out, 0 },
+        { sysfs_out, 4 },
+        { sysfs_out, 1 },
+        { sysfs_out, 5 },
+        { sysfs_out, 0 },
+    };
+    
+};