diff --git a/Tools/Frame_params/Parrot_Disco/start_ardupilot.sh b/Tools/Frame_params/Parrot_Disco/start_ardupilot.sh
index 261449fd03..3b1da0ffe8 100755
--- a/Tools/Frame_params/Parrot_Disco/start_ardupilot.sh
+++ b/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
diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index adc83cf9bd..42519c3315 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/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;
diff --git a/libraries/AP_Notify/DiscoLED.cpp b/libraries/AP_Notify/DiscoLED.cpp
new file mode 100644
index 0000000000..d82b3a94a9
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include
+#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
diff --git a/libraries/AP_Notify/DiscoLED.h b/libraries/AP_Notify/DiscoLED.h
new file mode 100644
index 0000000000..2d2d1caad5
--- /dev/null
+++ b/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 .
+*/
+#pragma once
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include
+#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