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 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include +#include #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 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include +#include #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