From 5e42800b5e6f5b6c41820eec016811b791e51147 Mon Sep 17 00:00:00 2001 From: Mathieu OTHACEHE Date: Wed, 26 Oct 2016 10:26:59 +0200 Subject: [PATCH] Disco: add LED support Use RGBLed generic implementation to support Disco LED. --- .../Parrot_Disco/start_ardupilot.sh | 3 + libraries/AP_Notify/AP_Notify.cpp | 4 ++ libraries/AP_Notify/DiscoLED.cpp | 62 +++++++++++++++++++ libraries/AP_Notify/DiscoLED.h | 39 ++++++++++++ 4 files changed, 108 insertions(+) create mode 100644 libraries/AP_Notify/DiscoLED.cpp create mode 100644 libraries/AP_Notify/DiscoLED.h 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