Disco: add LED support

Use RGBLed generic implementation to support Disco LED.

Tools/Frame_params/Parrot_Disco/start_ardupilot.sh

@@ -4,6 +4,9 @@ cd /data/ftp/internal_000/APM
 (
  date
 
+ # stop stock led daemon
+ pstop ledd
+
  # startup fan
  echo 1 > /sys/devices/platform/user_gpio/FAN/value
  

libraries/AP_Notify/AP_Notify.cpp

@@ -30,6 +30,7 @@
 #include "ToshibaLED_PX4.h"
 #include "VRBoard_LED.h"
 #include "DiscreteRGBLed.h"
+#include "DiscoLED.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Notify::var_info[] = {
@@ -124,6 +125,9 @@ struct AP_Notify::notify_events_type AP_Notify::events;
         AP_BoardLED boardled;
         RCOutputRGBLed bhled(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE);
         NotifyDevice *AP_Notify::_devices[] = {&boardled, &bhled};
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
+        DiscoLED discoled;
+        NotifyDevice *AP_Notify::_devices[] = {&discoled};
     #else
         AP_BoardLED boardled;
         ToshibaLED_I2C toshibaled;

libraries/AP_Notify/DiscoLED.cpp

@@ -0,0 +1,62 @@
+/*
+   Copyright (C) 2016 Mathieu Othacehe. All rights reserved.
+
+   This program 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 program 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 <AP_HAL_Linux/PWM_Sysfs.h>
+#include "DiscoLED.h"
+
+#define RED_PWM_INDEX   9
+#define GREEN_PWM_INDEX 8
+#define BLUE_PWM_INDEX  15
+
+#define DISCO_LED_LOW    0x33
+#define DISCO_LED_MEDIUM 0x7F
+#define DISCO_LED_HIGH   0xFF
+#define DISCO_LED_OFF    0x00
+
+DiscoLED::DiscoLED():
+    RGBLed(DISCO_LED_OFF, DISCO_LED_HIGH, DISCO_LED_MEDIUM, DISCO_LED_LOW)
+{
+}
+
+bool DiscoLED::hw_init()
+{
+    red_pwm = new Linux::PWM_Sysfs_Bebop(RED_PWM_INDEX);
+    green_pwm = new Linux::PWM_Sysfs_Bebop(GREEN_PWM_INDEX);
+    blue_pwm = new Linux::PWM_Sysfs_Bebop(BLUE_PWM_INDEX);
+
+    red_pwm_period = red_pwm->get_period();
+    green_pwm_period = green_pwm->get_period();
+    blue_pwm_period = blue_pwm->get_period();
+
+    red_pwm->enable(true);
+    green_pwm->enable(true);
+    blue_pwm->enable(true);
+
+    return true;
+}
+
+bool DiscoLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+    red_pwm->set_duty_cycle(red / UINT8_MAX * red_pwm_period);
+    green_pwm->set_duty_cycle(green / UINT8_MAX * green_pwm_period);
+    blue_pwm->set_duty_cycle(blue / UINT8_MAX * blue_pwm_period);
+
+    return true;
+}
+#endif

libraries/AP_Notify/DiscoLED.h

@@ -0,0 +1,39 @@
+/*
+  Copyright (C) 2016 Mathieu Othacehe. All rights reserved.
+
+  This program 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 program 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/>.
+*/
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include <AP_HAL_Linux/PWM_Sysfs.h>
+#include "RGBLed.h"
+
+class DiscoLED: public RGBLed
+{
+public:
+    DiscoLED();
+    bool hw_init(void);
+    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
+private:
+    Linux::PWM_Sysfs_Bebop *red_pwm;
+    Linux::PWM_Sysfs_Bebop *green_pwm;
+    Linux::PWM_Sysfs_Bebop *blue_pwm;
+
+    uint32_t red_pwm_period;
+    uint32_t green_pwm_period;
+    uint32_t blue_pwm_period;
+};
+#endif