mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-03 14:30:35 +08:00
Rename the 'device' directory back to 'drivers', it's less confusing that way.
Move the fmuv2 board driver out into the new world.
This commit is contained in:
px4dev
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# Board-specific startup code for the PX4FMUv2
|
||||
#
|
||||
|
||||
SRCS = px4fmu_can.c \
|
||||
px4fmu_init.c \
|
||||
px4fmu_pwm_servo.c \
|
||||
px4fmu_spi.c \
|
||||
px4fmu_usb.c
|
||||
@@ -0,0 +1,144 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_can.c
|
||||
*
|
||||
* Board-specific CAN functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/can.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "up_arch.h"
|
||||
|
||||
#include "stm32.h"
|
||||
#include "stm32_can.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
#ifdef CONFIG_CAN
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
/* Configuration ********************************************************************/
|
||||
|
||||
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
|
||||
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
|
||||
# undef CONFIG_STM32_CAN2
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_CAN1
|
||||
# define CAN_PORT 1
|
||||
#else
|
||||
# define CAN_PORT 2
|
||||
#endif
|
||||
|
||||
/* Debug ***************************************************************************/
|
||||
/* Non-standard debug that may be enabled just for testing CAN */
|
||||
|
||||
#ifdef CONFIG_DEBUG_CAN
|
||||
# define candbg dbg
|
||||
# define canvdbg vdbg
|
||||
# define canlldbg lldbg
|
||||
# define canllvdbg llvdbg
|
||||
#else
|
||||
# define candbg(x...)
|
||||
# define canvdbg(x...)
|
||||
# define canlldbg(x...)
|
||||
# define canllvdbg(x...)
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: can_devinit
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following interface to work with
|
||||
* examples/can.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int can_devinit(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
struct can_dev_s *can;
|
||||
int ret;
|
||||
|
||||
/* Check if we have already initialized */
|
||||
|
||||
if (!initialized) {
|
||||
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
||||
|
||||
can = stm32_caninitialize(CAN_PORT);
|
||||
|
||||
if (can == NULL) {
|
||||
candbg("ERROR: Failed to get CAN interface\n");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Register the CAN driver at "/dev/can0" */
|
||||
|
||||
ret = can_register("/dev/can0", can);
|
||||
|
||||
if (ret < 0) {
|
||||
candbg("ERROR: can_register failed: %d\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Now we are initialized */
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,229 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_init.c
|
||||
*
|
||||
* PX4FMU-specific early startup code. This file implements the
|
||||
* nsh_archinitialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialisation.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/spi.h>
|
||||
#include <nuttx/i2c.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
#include "stm32_uart.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_led.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Configuration ************************************************************/
|
||||
|
||||
/* Debug ********************************************************************/
|
||||
|
||||
#ifdef CONFIG_CPP_HAVE_VARARGS
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message(...) lowsyslog(__VA_ARGS__)
|
||||
# else
|
||||
# define message(...) printf(__VA_ARGS__)
|
||||
# endif
|
||||
#else
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message lowsyslog
|
||||
# else
|
||||
# define message printf
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the intitialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_boardinitialize(void)
|
||||
{
|
||||
/* configure SPI interfaces */
|
||||
stm32_spiinitialize();
|
||||
|
||||
/* configure LEDs */
|
||||
up_ledinit();
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: nsh_archinitialize
|
||||
*
|
||||
* Description:
|
||||
* Perform architecture specific initialization
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static struct spi_dev_s *spi1;
|
||||
static struct spi_dev_s *spi2;
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
__EXPORT int matherr(struct __exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#else
|
||||
__EXPORT int matherr(struct exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
__EXPORT int nsh_archinitialize(void)
|
||||
{
|
||||
|
||||
/* configure ADC pins */
|
||||
stm32_configgpio(GPIO_ADC1_IN10);
|
||||
stm32_configgpio(GPIO_ADC1_IN11);
|
||||
stm32_configgpio(GPIO_ADC1_IN12);
|
||||
|
||||
/* configure power supply control pins */
|
||||
stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
|
||||
stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN);
|
||||
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
|
||||
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
|
||||
|
||||
/* configure the high-resolution time/callout interface */
|
||||
hrt_init();
|
||||
|
||||
/* configure CPU load estimation */
|
||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
||||
cpuload_initialize_once();
|
||||
#endif
|
||||
|
||||
/* set up the serial DMA polling */
|
||||
static struct hrt_call serial_dma_call;
|
||||
struct timespec ts;
|
||||
|
||||
/*
|
||||
* Poll at 1ms intervals for received bytes that have not triggered
|
||||
* a DMA event.
|
||||
*/
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = 1000000;
|
||||
|
||||
hrt_call_every(&serial_dma_call,
|
||||
ts_to_abstime(&ts),
|
||||
ts_to_abstime(&ts),
|
||||
(hrt_callout)stm32_serial_dma_poll,
|
||||
NULL);
|
||||
|
||||
// initial LED state
|
||||
// XXX need to make this work again
|
||||
// drv_led_start();
|
||||
up_ledoff(LED_AMBER);
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
spi1 = up_spiinitialize(1);
|
||||
|
||||
if (!spi1) {
|
||||
message("[boot] FAILED to initialize SPI port 1\r\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
// Default SPI1 to 1MHz and de-assert the known chip selects.
|
||||
SPI_SETFREQUENCY(spi1, 10000000);
|
||||
SPI_SETBITS(spi1, 8);
|
||||
SPI_SETMODE(spi1, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
|
||||
up_udelay(20);
|
||||
|
||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
||||
|
||||
/* Get the SPI port for the FRAM */
|
||||
|
||||
message("[boot] Initializing SPI port 2\n");
|
||||
spi2 = up_spiinitialize(2);
|
||||
|
||||
if (!spi2) {
|
||||
message("[boot] FAILED to initialize SPI port 2\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
message("[boot] Successfully initialized SPI port 2\n");
|
||||
|
||||
/* XXX need a driver to bind the FRAM to */
|
||||
|
||||
//message("[boot] Successfully bound SPI port 2 to the FRAM driver\n");
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_internal.h
|
||||
*
|
||||
* PX4FMU internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/* these headers are not C++ safe */
|
||||
#include <stm32_internal.h>
|
||||
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************************************/
|
||||
/* Configuration ************************************************************************************/
|
||||
|
||||
/* PX4FMU GPIOs ***********************************************************************************/
|
||||
/* LEDs */
|
||||
|
||||
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
|
||||
|
||||
/* External interrupts */
|
||||
|
||||
/* SPI chip selects */
|
||||
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
|
||||
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
|
||||
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
|
||||
|
||||
/* User GPIOs
|
||||
*
|
||||
* GPIO0-5 are the PWM servo outputs.
|
||||
*/
|
||||
#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
|
||||
#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
|
||||
#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
|
||||
#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
|
||||
#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
|
||||
#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
|
||||
#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
|
||||
#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
|
||||
#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
|
||||
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
|
||||
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
|
||||
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
|
||||
|
||||
/* Power supply control and monitoring GPIOs */
|
||||
#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
|
||||
#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
|
||||
#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
|
||||
#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
|
||||
|
||||
/* USB OTG FS
|
||||
*
|
||||
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
|
||||
*/
|
||||
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public data
|
||||
****************************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void stm32_spiinitialize(void);
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
||||
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file px4fmu_pwm_servo.c
|
||||
*
|
||||
* Configuration data for the stm32 pwm_servo driver.
|
||||
*
|
||||
* Note that these arrays must always be fully-sized.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <drivers/stm32/drv_pwm_servo.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include <stm32_internal.h>
|
||||
#include <stm32_gpio.h>
|
||||
#include <stm32_tim.h>
|
||||
|
||||
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
|
||||
{
|
||||
.base = STM32_TIM1_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB2ENR_TIM1EN,
|
||||
.clock_freq = STM32_APB2_TIM1_CLKIN
|
||||
},
|
||||
{
|
||||
.base = STM32_TIM4_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM4EN,
|
||||
.clock_freq = STM32_APB1_TIM4_CLKIN
|
||||
}
|
||||
};
|
||||
|
||||
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH4OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 4,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH3OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH2OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM1_CH1OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 1,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM4_CH2OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM4_CH3OUT,
|
||||
.timer_index = 1,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1000,
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,141 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_spi.c
|
||||
*
|
||||
* Board-specific SPI functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "up_arch.h"
|
||||
#include "chip.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void weak_function stm32_spiinitialize(void)
|
||||
{
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
stm32_configgpio(GPIO_SPI_CS_GYRO);
|
||||
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
|
||||
stm32_configgpio(GPIO_SPI_CS_BARO);
|
||||
|
||||
/* De-activate all peripherals,
|
||||
* required for some peripheral
|
||||
* state machines
|
||||
*/
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
stm32_configgpio(GPIO_SPI_CS_FRAM);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
|
||||
#endif
|
||||
}
|
||||
|
||||
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* SPI select is active low, so write !selected to select the device */
|
||||
|
||||
switch (devid) {
|
||||
case PX4_SPIDEV_GYRO:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_ACCEL_MAG:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_BARO:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* there can only be one device on this bus, so always select it */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
/* FRAM is always present */
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_usb.c
|
||||
*
|
||||
* Board-specific USB functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/usb/usbdev.h>
|
||||
#include <nuttx/usb/usbdev_trace.h>
|
||||
|
||||
#include "up_arch.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Definitions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to setup USB-related GPIO pins for the PX4FMU board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_usbinitialize(void)
|
||||
{
|
||||
/* The OTG FS has an internal soft pull-up */
|
||||
|
||||
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
|
||||
|
||||
#ifdef CONFIG_STM32_OTGFS
|
||||
stm32_configgpio(GPIO_OTGFS_VBUS);
|
||||
/* XXX We only support device mode
|
||||
stm32_configgpio(GPIO_OTGFS_PWRON);
|
||||
stm32_configgpio(GPIO_OTGFS_OVER);
|
||||
*/
|
||||
#endif
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbsuspend
|
||||
*
|
||||
* Description:
|
||||
* Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
|
||||
* used. This function is called whenever the USB enters or leaves suspend mode.
|
||||
* This is an opportunity for the board logic to shutdown clocks, power, etc.
|
||||
* while the USB is suspended.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
|
||||
{
|
||||
ulldbg("resume: %d\n", resume);
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# LSM303D accel/mag driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = lsm303d
|
||||
SRCS = lsm303d.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# Interface driver for the PX4FMU board
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fmu
|
||||
SRCS = fmu.cpp
|
||||
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# TCA62724FMG driver for RGB LED
|
||||
#
|
||||
|
||||
MODULE_COMMAND = rgbled
|
||||
SRCS = rgbled.cpp
|
||||
@@ -0,0 +1,497 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rgbled.cpp
|
||||
*
|
||||
* Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <arch/board/board.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 <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "device/rgbled.h"
|
||||
|
||||
#define LED_ONTIME 120
|
||||
#define LED_OFFTIME 120
|
||||
|
||||
#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */
|
||||
#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */
|
||||
#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */
|
||||
#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */
|
||||
#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */
|
||||
#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/
|
||||
|
||||
#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
|
||||
#define SETTING_ENABLE 0x02 /**< on */
|
||||
|
||||
|
||||
enum ledModes {
|
||||
LED_MODE_TEST,
|
||||
LED_MODE_SYSTEMSTATE,
|
||||
LED_MODE_OFF
|
||||
};
|
||||
|
||||
class RGBLED : public device::I2C
|
||||
{
|
||||
public:
|
||||
RGBLED(int bus, int rgbled);
|
||||
virtual ~RGBLED();
|
||||
|
||||
|
||||
virtual int init();
|
||||
virtual int probe();
|
||||
virtual int info();
|
||||
virtual int setMode(enum ledModes mode);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
private:
|
||||
|
||||
enum ledColors {
|
||||
LED_COLOR_OFF,
|
||||
LED_COLOR_RED,
|
||||
LED_COLOR_YELLOW,
|
||||
LED_COLOR_PURPLE,
|
||||
LED_COLOR_GREEN,
|
||||
LED_COLOR_BLUE,
|
||||
LED_COLOR_WHITE,
|
||||
LED_COLOR_AMBER,
|
||||
};
|
||||
|
||||
enum ledBlinkModes {
|
||||
LED_BLINK_ON,
|
||||
LED_BLINK_OFF
|
||||
};
|
||||
|
||||
work_s _work;
|
||||
|
||||
int led_colors[8];
|
||||
int led_blink;
|
||||
|
||||
int mode;
|
||||
int running;
|
||||
|
||||
void setLEDColor(int ledcolor);
|
||||
static void led_trampoline(void *arg);
|
||||
void led();
|
||||
|
||||
int set(bool on, uint8_t r, uint8_t g, uint8_t b);
|
||||
int set_on(bool on);
|
||||
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
|
||||
int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
|
||||
};
|
||||
|
||||
/* for now, we only support one RGBLED */
|
||||
namespace
|
||||
{
|
||||
RGBLED *g_rgbled;
|
||||
}
|
||||
|
||||
|
||||
extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
|
||||
|
||||
RGBLED::RGBLED(int bus, int rgbled) :
|
||||
I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
|
||||
led_colors({0,0,0,0,0,0,0,0}),
|
||||
led_blink(LED_BLINK_OFF),
|
||||
mode(LED_MODE_OFF),
|
||||
running(false)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
RGBLED::~RGBLED()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::init()
|
||||
{
|
||||
int ret;
|
||||
ret = I2C::init();
|
||||
|
||||
if (ret != OK) {
|
||||
warnx("I2C init failed");
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* start off */
|
||||
set(false, 0, 0, 0);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::setMode(enum ledModes new_mode)
|
||||
{
|
||||
switch (new_mode) {
|
||||
case LED_MODE_SYSTEMSTATE:
|
||||
case LED_MODE_TEST:
|
||||
mode = new_mode;
|
||||
if (!running) {
|
||||
running = true;
|
||||
set_on(true);
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
|
||||
}
|
||||
break;
|
||||
case LED_MODE_OFF:
|
||||
|
||||
default:
|
||||
if (running) {
|
||||
running = false;
|
||||
set_on(false);
|
||||
}
|
||||
mode = LED_MODE_OFF;
|
||||
break;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::probe()
|
||||
{
|
||||
int ret;
|
||||
bool on, not_powersave;
|
||||
uint8_t r, g, b;
|
||||
|
||||
ret = get(on, not_powersave, r, g, b);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::info()
|
||||
{
|
||||
int ret;
|
||||
bool on, not_powersave;
|
||||
uint8_t r, g, b;
|
||||
|
||||
ret = get(on, not_powersave, r, g, b);
|
||||
|
||||
if (ret == OK) {
|
||||
/* we don't care about power-save mode */
|
||||
log("state: %s", on ? "ON" : "OFF");
|
||||
log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
|
||||
} else {
|
||||
warnx("failed to read led");
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = ENOTTY;
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RGBLED::led_trampoline(void *arg)
|
||||
{
|
||||
RGBLED *rgbl = (RGBLED *)arg;
|
||||
|
||||
rgbl->led();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
RGBLED::led()
|
||||
{
|
||||
static int led_thread_runcount=0;
|
||||
static int led_interval = 1000;
|
||||
|
||||
switch (mode) {
|
||||
case LED_MODE_TEST:
|
||||
/* Demo LED pattern for now */
|
||||
led_colors[0] = LED_COLOR_YELLOW;
|
||||
led_colors[1] = LED_COLOR_AMBER;
|
||||
led_colors[2] = LED_COLOR_RED;
|
||||
led_colors[3] = LED_COLOR_PURPLE;
|
||||
led_colors[4] = LED_COLOR_BLUE;
|
||||
led_colors[5] = LED_COLOR_GREEN;
|
||||
led_colors[6] = LED_COLOR_WHITE;
|
||||
led_colors[7] = LED_COLOR_OFF;
|
||||
led_blink = LED_BLINK_ON;
|
||||
break;
|
||||
|
||||
case LED_MODE_SYSTEMSTATE:
|
||||
/* XXX TODO set pattern */
|
||||
led_colors[0] = LED_COLOR_OFF;
|
||||
led_colors[1] = LED_COLOR_OFF;
|
||||
led_colors[2] = LED_COLOR_OFF;
|
||||
led_colors[3] = LED_COLOR_OFF;
|
||||
led_colors[4] = LED_COLOR_OFF;
|
||||
led_colors[5] = LED_COLOR_OFF;
|
||||
led_colors[6] = LED_COLOR_OFF;
|
||||
led_colors[7] = LED_COLOR_OFF;
|
||||
led_blink = LED_BLINK_OFF;
|
||||
|
||||
break;
|
||||
|
||||
case LED_MODE_OFF:
|
||||
default:
|
||||
return;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
if (led_thread_runcount & 1) {
|
||||
if (led_blink == LED_BLINK_ON)
|
||||
setLEDColor(LED_COLOR_OFF);
|
||||
led_interval = LED_OFFTIME;
|
||||
} else {
|
||||
setLEDColor(led_colors[(led_thread_runcount/2) % 8]);
|
||||
led_interval = LED_ONTIME;
|
||||
}
|
||||
|
||||
led_thread_runcount++;
|
||||
|
||||
|
||||
if(running) {
|
||||
/* re-queue ourselves to run again later */
|
||||
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval);
|
||||
} else {
|
||||
set_on(false);
|
||||
}
|
||||
}
|
||||
|
||||
void RGBLED::setLEDColor(int ledcolor) {
|
||||
switch (ledcolor) {
|
||||
case LED_COLOR_OFF: // off
|
||||
set_rgb(0,0,0);
|
||||
break;
|
||||
case LED_COLOR_RED: // red
|
||||
set_rgb(255,0,0);
|
||||
break;
|
||||
case LED_COLOR_YELLOW: // yellow
|
||||
set_rgb(255,70,0);
|
||||
break;
|
||||
case LED_COLOR_PURPLE: // purple
|
||||
set_rgb(255,0,255);
|
||||
break;
|
||||
case LED_COLOR_GREEN: // green
|
||||
set_rgb(0,255,0);
|
||||
break;
|
||||
case LED_COLOR_BLUE: // blue
|
||||
set_rgb(0,0,255);
|
||||
break;
|
||||
case LED_COLOR_WHITE: // white
|
||||
set_rgb(255,255,255);
|
||||
break;
|
||||
case LED_COLOR_AMBER: // amber
|
||||
set_rgb(255,20,0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
uint8_t settings_byte = 0;
|
||||
|
||||
if (on)
|
||||
settings_byte |= SETTING_ENABLE;
|
||||
/* powersave not used */
|
||||
// if (not_powersave)
|
||||
settings_byte |= SETTING_NOT_POWERSAVE;
|
||||
|
||||
const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
|
||||
|
||||
return transfer(msg, sizeof(msg), nullptr, 0);
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::set_on(bool on)
|
||||
{
|
||||
uint8_t settings_byte = 0;
|
||||
|
||||
if (on)
|
||||
settings_byte |= SETTING_ENABLE;
|
||||
|
||||
/* powersave not used */
|
||||
// if (not_powersave)
|
||||
settings_byte |= SETTING_NOT_POWERSAVE;
|
||||
|
||||
const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
|
||||
|
||||
return transfer(msg, sizeof(msg), nullptr, 0);
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
|
||||
|
||||
return transfer(msg, sizeof(msg), nullptr, 0);
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
|
||||
{
|
||||
uint8_t result[2];
|
||||
int ret;
|
||||
|
||||
ret = transfer(nullptr, 0, &result[0], 2);
|
||||
|
||||
if (ret == OK) {
|
||||
on = result[0] & SETTING_ENABLE;
|
||||
not_powersave = result[0] & SETTING_NOT_POWERSAVE;
|
||||
/* XXX check, looks wrong */
|
||||
r = (result[0] & 0x0f)*255/15;
|
||||
g = (result[1] & 0xf0)*255/15;
|
||||
b = (result[1] & 0x0f)*255/15;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void rgbled_usage();
|
||||
|
||||
|
||||
void rgbled_usage() {
|
||||
warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'");
|
||||
warnx("options:");
|
||||
warnx("\t-b --bus i2cbus (3)");
|
||||
warnx("\t-a --ddr addr (9)");
|
||||
}
|
||||
|
||||
int
|
||||
rgbled_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
int i2cdevice = PX4_I2C_BUS_LED;
|
||||
int rgbledadr = ADDR; /* 7bit */
|
||||
|
||||
int x;
|
||||
|
||||
for (x = 1; x < argc; x++) {
|
||||
if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
|
||||
if (argc > x + 1) {
|
||||
i2cdevice = atoi(argv[x + 1]);
|
||||
}
|
||||
}
|
||||
|
||||
if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) {
|
||||
if (argc > x + 1) {
|
||||
rgbledadr = atoi(argv[x + 1]);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
if (g_rgbled != nullptr)
|
||||
errx(1, "already started");
|
||||
|
||||
g_rgbled = new RGBLED(i2cdevice, rgbledadr);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
if (g_rgbled == nullptr) {
|
||||
fprintf(stderr, "not started\n");
|
||||
rgbled_usage();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
g_rgbled->setMode(LED_MODE_TEST);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "systemstate")) {
|
||||
g_rgbled->setMode(LED_MODE_SYSTEMSTATE);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "info")) {
|
||||
g_rgbled->info();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "off")) {
|
||||
g_rgbled->setMode(LED_MODE_OFF);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
/* things that require access to the device */
|
||||
int fd = open(RGBLED_DEVICE_PATH, 0);
|
||||
if (fd < 0)
|
||||
err(1, "can't open RGBLED device");
|
||||
|
||||
rgbled_usage();
|
||||
exit(0);
|
||||
}
|
||||
Reference in New Issue
Block a user