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

@@ -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;
 }

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