From 701d91f20ca07fcc7ab22a05344fbf7a88fa32ed Mon Sep 17 00:00:00 2001
From: Mathieu OTHACEHE <mathieu.othacehe@parrot.com>
Date: Wed, 4 Jan 2017 09:25:07 +0100
Subject: [PATCH] AP_Notify: Disco: use new led sysfs backend or, if not
 available, legacy pwm backend to drive leds.

In new Disco releases, the led sysfs api will replace the pwm sysfs
api to drive the tricolor led.

Keep pwm sysfs api for compatibility reasons.
---
 libraries/AP_Notify/DiscoLED.cpp | 38 ++++++++++++++++++++++++++++----
 libraries/AP_Notify/DiscoLED.h   | 12 ++++++++++
 2 files changed, 46 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Notify/DiscoLED.cpp b/libraries/AP_Notify/DiscoLED.cpp
index 3dbdd2e766..8399e6a636 100644
--- a/libraries/AP_Notify/DiscoLED.cpp
+++ b/libraries/AP_Notify/DiscoLED.cpp
@@ -18,6 +18,7 @@
 #include <AP_HAL/AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include <AP_HAL_Linux/PWM_Sysfs.h>
+#include <AP_HAL_Linux/Led_Sysfs.h>
 #include "DiscoLED.h"
 
 #define RED_PWM_INDEX   9
@@ -29,16 +30,33 @@
 #define DISCO_LED_HIGH   0xFF
 #define DISCO_LED_OFF    0x00
 
+#define DISCO_LED_RED_NAME   "evinrude:red"
+#define DISCO_LED_GREEN_NAME "evinrude:green"
+#define DISCO_LED_BLUE_NAME  "evinrude:blue"
+
 DiscoLED::DiscoLED():
     RGBLed(DISCO_LED_OFF, DISCO_LED_HIGH, DISCO_LED_MEDIUM, DISCO_LED_LOW),
     red_pwm(RED_PWM_INDEX),
     green_pwm(GREEN_PWM_INDEX),
-    blue_pwm(BLUE_PWM_INDEX)
+    blue_pwm(BLUE_PWM_INDEX),
+
+    red_led(DISCO_LED_RED_NAME),
+    green_led(DISCO_LED_GREEN_NAME),
+    blue_led(DISCO_LED_BLUE_NAME)
 {
 }
 
 bool DiscoLED::hw_init()
 {
+    /* If led sysfs api is present, use it, else use pwm sysfs api to
+       drive Disco leds */
+    if (red_led.init() && green_led.init() && blue_led.init()) {
+        backend = LED_SYSFS;
+        return true;
+    }
+
+    backend = PWM_SYSFS;
+
     red_pwm_period = red_pwm.get_period();
     green_pwm_period = green_pwm.get_period();
     blue_pwm_period = blue_pwm.get_period();
@@ -56,9 +74,21 @@ bool DiscoLED::hw_init()
 
 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);
+
+    switch (backend) {
+    case PWM_SYSFS:
+        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);
+        break;
+    case LED_SYSFS:
+        red_led.set_brightness(red);
+        green_led.set_brightness(green);
+        blue_led.set_brightness(blue);
+        break;
+    default:
+        return false;
+    }
 
     return true;
 }
diff --git a/libraries/AP_Notify/DiscoLED.h b/libraries/AP_Notify/DiscoLED.h
index 32cf82a104..760d0960fa 100644
--- a/libraries/AP_Notify/DiscoLED.h
+++ b/libraries/AP_Notify/DiscoLED.h
@@ -19,6 +19,7 @@
 #include <AP_HAL/AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include <AP_HAL_Linux/PWM_Sysfs.h>
+#include <AP_HAL_Linux/Led_Sysfs.h>
 #include "RGBLed.h"
 
 class DiscoLED: public RGBLed
@@ -32,8 +33,19 @@ private:
     Linux::PWM_Sysfs_Bebop green_pwm;
     Linux::PWM_Sysfs_Bebop blue_pwm;
 
+    Linux::Led_Sysfs red_led;
+    Linux::Led_Sysfs green_led;
+    Linux::Led_Sysfs blue_led;
+
     uint32_t red_pwm_period;
     uint32_t green_pwm_period;
     uint32_t blue_pwm_period;
+
+    enum led_backend {
+        LED_SYSFS,
+        PWM_SYSFS
+    };
+
+    enum led_backend backend;
 };
 #endif