mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 04:33:35 +08:00
supports MindPXv2 borad which is a product from AirMind.
This commit is contained in:
Felix Hu
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__rgbled_pwm
|
||||
MAIN rgbled
|
||||
COMPILE_FLAGS
|
||||
-Os
|
||||
SRCS
|
||||
drv_led_pwm.cpp
|
||||
rgbled_pwm.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -0,0 +1,382 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name Airmind nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_led_pwm.cpp
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/stm32/drv_io_timer.h>
|
||||
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
|
||||
#define REG(_tmr, _reg) (*(volatile uint32_t *)(led_pwm_timers[_tmr].base + _reg))
|
||||
|
||||
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
|
||||
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
|
||||
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
|
||||
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
|
||||
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
|
||||
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
|
||||
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
|
||||
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
|
||||
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
|
||||
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
|
||||
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
|
||||
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
|
||||
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
|
||||
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
|
||||
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
|
||||
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
|
||||
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
|
||||
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
|
||||
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
|
||||
|
||||
static void led_pwm_timer_init(unsigned timer);
|
||||
static void led_pwm_timer_set_rate(unsigned timer, unsigned rate);
|
||||
static void led_pwm_channel_init(unsigned channel);
|
||||
|
||||
int led_pwm_servo_set(unsigned channel, uint8_t value);
|
||||
unsigned led_pwm_servo_get(unsigned channel);
|
||||
int led_pwm_servo_init(void);
|
||||
void led_pwm_servo_deinit(void);
|
||||
void led_pwm_servo_arm(bool armed);
|
||||
unsigned led_pwm_timer_get_period(unsigned timer);
|
||||
|
||||
|
||||
const struct io_timers_t led_pwm_timers[3] = {
|
||||
{
|
||||
.base = STM32_TIM3_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM3EN,
|
||||
.clock_freq = STM32_APB1_TIM3_CLKIN,
|
||||
.vectorno = STM32_IRQ_TIM3,
|
||||
.first_channel_index = 4,
|
||||
.last_channel_index = 7,
|
||||
.handler = io_timer_handler0,
|
||||
},
|
||||
{
|
||||
.base = STM32_TIM3_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM3EN,
|
||||
.clock_freq = STM32_APB1_TIM3_CLKIN,
|
||||
.vectorno = STM32_IRQ_TIM3,
|
||||
.first_channel_index = 4,
|
||||
.last_channel_index = 7,
|
||||
.handler = io_timer_handler1,
|
||||
},
|
||||
{
|
||||
.base = STM32_TIM3_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM3EN,
|
||||
.clock_freq = STM32_APB1_TIM3_CLKIN,
|
||||
.vectorno = STM32_IRQ_TIM3,
|
||||
.first_channel_index = 4,
|
||||
.last_channel_index = 7,
|
||||
.handler = io_timer_handler2,
|
||||
}
|
||||
};
|
||||
|
||||
#define LED_TIM3_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0)
|
||||
#define LED_TIM3_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7)
|
||||
#define LED_TIM3_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1)
|
||||
#define LED_TIM3_CH3IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0)
|
||||
#define LED_TIM3_CH2IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7)
|
||||
#define LED_TIM3_CH4IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1)
|
||||
|
||||
const struct timer_io_channels_t led_pwm_channels[3] = {
|
||||
{
|
||||
.gpio_out = LED_TIM3_CH3OUT,
|
||||
.gpio_in = LED_TIM3_CH3IN,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 3,
|
||||
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF,
|
||||
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
|
||||
},
|
||||
{
|
||||
.gpio_out = LED_TIM3_CH2OUT,
|
||||
.gpio_in = LED_TIM3_CH2IN,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 2,
|
||||
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF,
|
||||
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
|
||||
},
|
||||
{
|
||||
.gpio_out = LED_TIM3_CH4OUT,
|
||||
.gpio_in = LED_TIM3_CH4IN,
|
||||
.timer_index = 2,
|
||||
.timer_channel = 4,
|
||||
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF,
|
||||
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
static void
|
||||
led_pwm_timer_init(unsigned timer)
|
||||
{
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit);
|
||||
|
||||
/* disable and configure the timer */
|
||||
rCR1(timer) = 0;
|
||||
rCR2(timer) = 0;
|
||||
rSMCR(timer) = 0;
|
||||
rDIER(timer) = 0;
|
||||
rCCER(timer) = 0;
|
||||
rCCMR1(timer) = 0;
|
||||
rCCMR2(timer) = 0;
|
||||
rCCER(timer) = 0;
|
||||
rDCR(timer) = 0;
|
||||
|
||||
if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) {
|
||||
/* master output enable = on */
|
||||
rBDTR(timer) = ATIM_BDTR_MOE;
|
||||
}
|
||||
|
||||
/* configure the timer to free-run at 1MHz */
|
||||
rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1;
|
||||
|
||||
/* default to updating at 50Hz */
|
||||
led_pwm_timer_set_rate(timer, 50);
|
||||
|
||||
/* note that the timer is left disabled - arming is performed separately */
|
||||
}
|
||||
unsigned
|
||||
led_pwm_timer_get_period(unsigned timer)
|
||||
{
|
||||
return (rARR(timer));
|
||||
}
|
||||
static void
|
||||
led_pwm_timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
/* configure the timer to update at the desired rate */
|
||||
rARR(timer) = 1000000 / rate;
|
||||
|
||||
/* generate an update event; reloads the counter and all registers */
|
||||
rEGR(timer) = GTIM_EGR_UG;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
led_pwm_channel_init(unsigned channel)
|
||||
{
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
|
||||
/* configure the GPIO first */
|
||||
stm32_configgpio(led_pwm_channels[channel].gpio_out);
|
||||
|
||||
/* configure the channel */
|
||||
switch (led_pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
|
||||
//rCCR1(timer) = led_pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= GTIM_CCER_CC1E;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
|
||||
//rCCR2(timer) = led_pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= GTIM_CCER_CC2E;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
|
||||
//rCCR3(timer) = led_pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= GTIM_CCER_CC3E;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
|
||||
//rCCR4(timer) = led_pwm_channels[channel].default_value;
|
||||
rCCER(timer) |= GTIM_CCER_CC4E;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
led_pwm_servo_set(unsigned channel, uint8_t cvalue)
|
||||
{
|
||||
if (channel >= 3) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((led_pwm_timers[timer].base == 0) ||
|
||||
(led_pwm_channels[channel].gpio_out== 0)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned period = led_pwm_timer_get_period(timer);
|
||||
|
||||
unsigned value = period - (unsigned)cvalue * period / 255;
|
||||
|
||||
/* configure the channel */
|
||||
if (value > 0) {
|
||||
value--;
|
||||
}
|
||||
|
||||
|
||||
switch (led_pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
rCCR1(timer) = value;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
rCCR2(timer) = value;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rCCR3(timer) = value;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
rCCR4(timer) = value;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
unsigned
|
||||
led_pwm_servo_get(unsigned channel)
|
||||
{
|
||||
if (channel >= 3) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
servo_position_t value = 0;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((led_pwm_timers[timer].base == 0) ||
|
||||
(led_pwm_channels[channel].timer_channel == 0)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* configure the channel */
|
||||
switch (led_pwm_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
value = rCCR1(timer);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
value = rCCR2(timer);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
value = rCCR3(timer);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
value = rCCR4(timer);
|
||||
break;
|
||||
}
|
||||
|
||||
unsigned period = led_pwm_timer_get_period(timer);
|
||||
return ((value + 1) * 255 / period);
|
||||
}
|
||||
int
|
||||
led_pwm_servo_init(void)
|
||||
{
|
||||
/* do basic timer initialisation first */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
led_pwm_timer_init(i);
|
||||
}
|
||||
|
||||
/* now init channels */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
led_pwm_channel_init(i);
|
||||
}
|
||||
|
||||
led_pwm_servo_arm(true);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
led_pwm_servo_deinit(void)
|
||||
{
|
||||
/* disable the timers */
|
||||
led_pwm_servo_arm(false);
|
||||
}
|
||||
void
|
||||
led_pwm_servo_arm(bool armed)
|
||||
{
|
||||
/* iterate timers and arm/disarm appropriately */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (led_pwm_timers[i].base != 0) {
|
||||
if (armed) {
|
||||
/* force an update to preload all registers */
|
||||
rEGR(i) = GTIM_EGR_UG;
|
||||
|
||||
/* arm requires the timer be enabled */
|
||||
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
|
||||
|
||||
} else {
|
||||
// XXX This leads to FMU PWM being still active
|
||||
// but uncontrollable. Just disable the timer
|
||||
// and risk a runt.
|
||||
///* on disarm, just stop auto-reload so we don't generate runts */
|
||||
//rCR1(i) &= ~GTIM_CR1_ARPE;
|
||||
rCR1(i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,697 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name Airmind nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rgbled_pwm.cpp
|
||||
*
|
||||
* Driver for the onboard RGB LED controller by PWM.
|
||||
* this driver is based the PX4 led driver
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
//#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include <drivers/drv_rgbled.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#define RGBLED_ONTIME 120
|
||||
#define RGBLED_OFFTIME 120
|
||||
|
||||
class RGBLED_PWM : public device::CDev
|
||||
{
|
||||
public:
|
||||
RGBLED_PWM();
|
||||
virtual ~RGBLED_PWM();
|
||||
|
||||
|
||||
virtual int init();
|
||||
virtual int probe();
|
||||
virtual int info();
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
private:
|
||||
struct hrt_call _call;
|
||||
struct hrt_call _call_r;
|
||||
struct hrt_call _call_g;
|
||||
struct hrt_call _call_b;
|
||||
|
||||
work_s _work;
|
||||
|
||||
rgbled_mode_t _mode;
|
||||
rgbled_pattern_t _pattern;
|
||||
|
||||
uint8_t _r;
|
||||
uint8_t _g;
|
||||
uint8_t _b;
|
||||
bool _enable;
|
||||
float _brightness;
|
||||
|
||||
bool _running;
|
||||
int _led_interval;
|
||||
bool _should_run;
|
||||
int _counter;
|
||||
|
||||
static void pwm_begin(void *arg);
|
||||
static void pwm_end_r(void *arg);
|
||||
static void pwm_end_g(void *arg);
|
||||
static void pwm_end_b(void *arg);
|
||||
|
||||
void set_color(rgbled_color_t ledcolor);
|
||||
void set_mode(rgbled_mode_t mode);
|
||||
void set_pattern(rgbled_pattern_t *pattern);
|
||||
|
||||
static void led_trampoline(void *arg);
|
||||
void led();
|
||||
|
||||
int send_led_enable(bool enable);
|
||||
int send_led_rgb();
|
||||
int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
|
||||
};
|
||||
|
||||
extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
|
||||
extern int led_pwm_servo_set(unsigned channel, uint8_t value);
|
||||
extern unsigned led_pwm_servo_get(unsigned channel);
|
||||
extern int led_pwm_servo_init(void);
|
||||
extern void led_pwm_servo_deinit(void);
|
||||
|
||||
|
||||
/* for now, we only support one RGBLED */
|
||||
namespace
|
||||
{
|
||||
RGBLED_PWM *g_rgbled = nullptr;
|
||||
}
|
||||
|
||||
void rgbled_usage();
|
||||
|
||||
RGBLED_PWM::RGBLED_PWM() :
|
||||
CDev("rgbled", RGBLED0_DEVICE_PATH),
|
||||
_call{},
|
||||
_call_r{},
|
||||
_call_g{},
|
||||
_call_b{},
|
||||
_mode(RGBLED_MODE_OFF),
|
||||
_r(0),
|
||||
_g(0),
|
||||
_b(0),
|
||||
_enable(false),
|
||||
_brightness(1.0f),
|
||||
_running(false),
|
||||
_led_interval(0),
|
||||
_should_run(false),
|
||||
_counter(0)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
memset(&_pattern, 0, sizeof(_pattern));
|
||||
|
||||
memset(&_call, 0, sizeof(_call));
|
||||
memset(&_call_r, 0, sizeof(_call_r));
|
||||
memset(&_call_g, 0, sizeof(_call_g));
|
||||
memset(&_call_b, 0, sizeof(_call_b));
|
||||
}
|
||||
|
||||
RGBLED_PWM::~RGBLED_PWM()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_PWM::init()
|
||||
{
|
||||
/* switch off LED on start */
|
||||
CDev::init();
|
||||
#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
|
||||
led_pwm_servo_init();
|
||||
send_led_enable(false);
|
||||
send_led_rgb();
|
||||
#endif
|
||||
return OK;
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_begin(void *arg)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RGBLED_PWM::pwm_end_r(void *arg)
|
||||
{
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_end_g(void *arg)
|
||||
{
|
||||
}
|
||||
void
|
||||
RGBLED_PWM::pwm_end_b(void *arg)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
RGBLED_PWM::info()
|
||||
{
|
||||
int ret;
|
||||
bool on, powersave;
|
||||
uint8_t r, g, b;
|
||||
|
||||
ret = get(on, powersave, r, g, b);
|
||||
|
||||
if (ret == OK) {
|
||||
/* we don't care about power-save mode */
|
||||
DEVICE_LOG("state: %s", on ? "ON" : "OFF");
|
||||
DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
|
||||
|
||||
} else {
|
||||
warnx("failed to read led");
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
int
|
||||
RGBLED_PWM::probe()
|
||||
{
|
||||
return (OK);
|
||||
}
|
||||
int
|
||||
RGBLED_PWM::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = ENOTTY;
|
||||
|
||||
switch (cmd) {
|
||||
case RGBLED_SET_RGB:
|
||||
/* set the specified color */
|
||||
_r = ((rgbled_rgbset_t *) arg)->red;
|
||||
_g = ((rgbled_rgbset_t *) arg)->green;
|
||||
_b = ((rgbled_rgbset_t *) arg)->blue;
|
||||
send_led_rgb();
|
||||
return OK;
|
||||
|
||||
case RGBLED_SET_COLOR:
|
||||
/* set the specified color name */
|
||||
set_color((rgbled_color_t)arg);
|
||||
send_led_rgb();
|
||||
return OK;
|
||||
|
||||
case RGBLED_SET_MODE:
|
||||
/* set the specified mode */
|
||||
set_mode((rgbled_mode_t)arg);
|
||||
return OK;
|
||||
|
||||
case RGBLED_SET_PATTERN:
|
||||
/* set a special pattern */
|
||||
set_pattern((rgbled_pattern_t *)arg);
|
||||
return OK;
|
||||
|
||||
default:
|
||||
/* see if the parent class can make any use of it */
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RGBLED_PWM::led_trampoline(void *arg)
|
||||
{
|
||||
RGBLED_PWM *rgbl = reinterpret_cast<RGBLED_PWM *>(arg);
|
||||
|
||||
rgbl->led();
|
||||
}
|
||||
|
||||
/**
|
||||
* Main loop function
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::led()
|
||||
{
|
||||
if (!_should_run) {
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case RGBLED_MODE_BLINK_SLOW:
|
||||
case RGBLED_MODE_BLINK_NORMAL:
|
||||
case RGBLED_MODE_BLINK_FAST:
|
||||
if (_counter >= 2) {
|
||||
_counter = 0;
|
||||
}
|
||||
|
||||
send_led_enable(_counter == 0);
|
||||
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_BREATHE:
|
||||
|
||||
if (_counter >= 62) {
|
||||
_counter = 0;
|
||||
}
|
||||
|
||||
int n;
|
||||
|
||||
if (_counter < 32) {
|
||||
n = _counter;
|
||||
|
||||
} else {
|
||||
n = 62 - _counter;
|
||||
}
|
||||
|
||||
_brightness = n * n / (31.0f * 31.0f);
|
||||
// send_led_rgb();
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_PATTERN:
|
||||
|
||||
/* don't run out of the pattern array and stop if the next frame is 0 */
|
||||
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) {
|
||||
_counter = 0;
|
||||
}
|
||||
|
||||
set_color(_pattern.color[_counter]);
|
||||
send_led_rgb();
|
||||
_led_interval = _pattern.duration[_counter];
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
_counter++;
|
||||
|
||||
/* re-queue ourselves to run again later */
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, _led_interval);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse color constant and set _r _g _b values
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_color(rgbled_color_t color)
|
||||
{
|
||||
switch (color) {
|
||||
case RGBLED_COLOR_OFF:
|
||||
_r = 0;
|
||||
_g = 0;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_RED:
|
||||
_r = 255;
|
||||
_g = 0;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_YELLOW:
|
||||
_r = 255;
|
||||
_g = 200;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_PURPLE:
|
||||
_r = 255;
|
||||
_g = 0;
|
||||
_b = 255;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_GREEN:
|
||||
_r = 0;
|
||||
_g = 255;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_BLUE:
|
||||
_r = 0;
|
||||
_g = 0;
|
||||
_b = 255;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_WHITE:
|
||||
_r = 255;
|
||||
_g = 255;
|
||||
_b = 255;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_AMBER:
|
||||
_r = 255;
|
||||
_g = 80;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_RED:
|
||||
_r = 90;
|
||||
_g = 0;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_YELLOW:
|
||||
_r = 80;
|
||||
_g = 30;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_PURPLE:
|
||||
_r = 45;
|
||||
_g = 0;
|
||||
_b = 45;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_GREEN:
|
||||
_r = 0;
|
||||
_g = 90;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_BLUE:
|
||||
_r = 0;
|
||||
_g = 0;
|
||||
_b = 90;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_WHITE:
|
||||
_r = 30;
|
||||
_g = 30;
|
||||
_b = 30;
|
||||
break;
|
||||
|
||||
case RGBLED_COLOR_DIM_AMBER:
|
||||
_r = 80;
|
||||
_g = 20;
|
||||
_b = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("color unknown");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_mode(rgbled_mode_t mode)
|
||||
{
|
||||
if (mode != _mode) {
|
||||
_mode = mode;
|
||||
|
||||
switch (mode) {
|
||||
case RGBLED_MODE_OFF:
|
||||
_should_run = false;
|
||||
send_led_enable(false);
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_ON:
|
||||
_brightness = 1.0f;
|
||||
send_led_rgb();
|
||||
send_led_enable(true);
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_BLINK_SLOW:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_led_interval = 2000;
|
||||
_brightness = 1.0f;
|
||||
send_led_rgb();
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_BLINK_NORMAL:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_led_interval = 500;
|
||||
_brightness = 1.0f;
|
||||
send_led_rgb();
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_BLINK_FAST:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_led_interval = 100;
|
||||
_brightness = 1.0f;
|
||||
send_led_rgb();
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_BREATHE:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_led_interval = 25;
|
||||
send_led_enable(true);
|
||||
break;
|
||||
|
||||
case RGBLED_MODE_PATTERN:
|
||||
_should_run = true;
|
||||
_counter = 0;
|
||||
_brightness = 1.0f;
|
||||
send_led_enable(true);
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("mode unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
/* if it should run now, start the workq */
|
||||
if (_should_run && !_running) {
|
||||
_running = true;
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 1);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set pattern for PATTERN mode, but don't change current mode
|
||||
*/
|
||||
void
|
||||
RGBLED_PWM::set_pattern(rgbled_pattern_t *pattern)
|
||||
{
|
||||
memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sent ENABLE flag to LED driver
|
||||
*/
|
||||
int
|
||||
RGBLED_PWM::send_led_enable(bool enable)
|
||||
{
|
||||
_enable = enable;
|
||||
send_led_rgb();
|
||||
return (OK);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send RGB PWM settings to LED driver according to current color and brightness
|
||||
*/
|
||||
int
|
||||
RGBLED_PWM::send_led_rgb()
|
||||
{
|
||||
#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
|
||||
|
||||
if (_enable) {
|
||||
led_pwm_servo_set(0, _r);
|
||||
led_pwm_servo_set(1, _g);
|
||||
led_pwm_servo_set(2, _b);
|
||||
|
||||
} else {
|
||||
led_pwm_servo_set(0, 0);
|
||||
led_pwm_servo_set(1, 0);
|
||||
led_pwm_servo_set(2, 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
return (OK);
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
|
||||
{
|
||||
powersave = OK; on = _enable;
|
||||
r = _r;
|
||||
g = _g;
|
||||
b = _b;
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
rgbled_usage()
|
||||
{
|
||||
warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
|
||||
}
|
||||
|
||||
int
|
||||
rgbled_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'a':
|
||||
break;
|
||||
|
||||
case 'b':
|
||||
break;
|
||||
|
||||
default:
|
||||
rgbled_usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (optind >= argc) {
|
||||
rgbled_usage();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (g_rgbled != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
if (g_rgbled == nullptr) {
|
||||
g_rgbled = new RGBLED_PWM();
|
||||
|
||||
if (g_rgbled == nullptr) {
|
||||
errx(1, "new failed");
|
||||
}
|
||||
|
||||
if (OK != g_rgbled->init()) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
errx(1, "init failed");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/* need the driver past this point */
|
||||
if (g_rgbled == nullptr) {
|
||||
warnx("not started");
|
||||
rgbled_usage();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test")) {
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
|
||||
{500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern
|
||||
};
|
||||
|
||||
ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
|
||||
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
|
||||
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "info")) {
|
||||
g_rgbled->info();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
|
||||
close(fd);
|
||||
|
||||
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "rgb")) {
|
||||
if (argc < 5) {
|
||||
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
|
||||
}
|
||||
|
||||
fd = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
rgbled_rgbset_t v;
|
||||
v.red = strtol(argv[2], NULL, 0);
|
||||
v.green = strtol(argv[3], NULL, 0);
|
||||
v.blue = strtol(argv[4], NULL, 0);
|
||||
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
|
||||
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
rgbled_usage();
|
||||
exit(0);
|
||||
}
|
||||
Reference in New Issue
Block a user