From 0eddcb335707284269cf9c89ac6b820efa1a6b13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 10:56:58 -0700 Subject: [PATCH 1/3] Tried to collect some changes that I needed to build for FMUv2 into a commit --- makefiles/config_px4fmu_default.mk | 5 +- nuttx/configs/px4fmu/include/board.h | 6 + nuttx/configs/px4fmu/nsh/appconfig | 5 +- nuttx/configs/px4fmu/nsh/defconfig | 2 +- src/drivers/boards/px4fmu/module.mk | 9 + src/drivers/boards/px4fmu/px4fmu_can.c | 144 ++++++++++ src/drivers/boards/px4fmu/px4fmu_init.c | 266 +++++++++++++++++++ src/drivers/boards/px4fmu/px4fmu_internal.h | 162 +++++++++++ src/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 87 ++++++ src/drivers/boards/px4fmu/px4fmu_spi.c | 154 +++++++++++ src/drivers/boards/px4fmu/px4fmu_usb.c | 108 ++++++++ 11 files changed, 942 insertions(+), 6 deletions(-) create mode 100644 src/drivers/boards/px4fmu/module.mk create mode 100644 src/drivers/boards/px4fmu/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_init.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_internal.h create mode 100644 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_usb.c diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1a6c91b269..81dd202e79 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -10,7 +10,9 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += drivers/px4fmu +MODULES += drivers/boards/px4fmu +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 # # Transitional support - add commands from the NuttX export archive. @@ -44,7 +46,6 @@ BUILTIN_COMMANDS := \ $(call _B, hmc5883, , 4096, hmc5883_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 0bc0b30216..294b6c3984 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -308,6 +308,10 @@ #define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 #define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_2 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + #define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 #define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 #define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 @@ -321,6 +325,8 @@ #define PX4_SPIDEV_ACCEL 2 #define PX4_SPIDEV_MPU 3 +#define PX4_SPIDEV_ACCEL_MAG 2 // external for anti vibration test + /* * Tone alarm output */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index d642b46923..b60bbfdd92 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -110,16 +110,15 @@ endif CONFIGURED_APPS += systemcmds/i2c # Communication and Drivers -CONFIGURED_APPS += drivers/boards/px4fmu +#CONFIGURED_APPS += drivers/boards/px4fmu CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 -CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 -CONFIGURED_APPS += drivers/led +#CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 130886aac2..cf30b835fc 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -85,7 +85,7 @@ CONFIG_ARCH_FPU=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=y +CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_DMA=y diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk new file mode 100644 index 0000000000..2cb261d305 --- /dev/null +++ b/src/drivers/boards/px4fmu/module.mk @@ -0,0 +1,9 @@ +# +# Board-specific startup code for the PX4FMU +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c new file mode 100644 index 0000000000..86ba183b83 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_can.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 + +#include +#include + +#include +#include + +#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 \ No newline at end of file diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 0000000000..96c7fa2df3 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * 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; +static struct spi_dev_s *spi3; + +#include + +#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) +{ + int result; + +// /* 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 +// drv_led_start(); + up_ledoff(LED_BLUE); + up_ledoff(LED_AMBER); + up_ledon(LED_BLUE); + + /* 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_ACCEL, false); +// SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); +// SPI_SELECT(spi2, PX4_SPIDEV_ACCEL, false); +// SPI_SELECT(spi2, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port2\r\n"); + + /* Get the SPI port for the microSD slot */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); +// stm32_configgpio(GPIO_ADC1_IN12); +// stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + + return OK; +} diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 0000000000..671a58f8f6 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +//#ifdef CONFIG_STM32_SPI2 +//# error "SPI2 is not supported on this board" +//#endif + +#if defined(CONFIG_STM32_CAN1) +# warning "CAN1 is not supported on this board" +#endif + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + +/* External interrupts */ +#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +// XXX MPU6000 DRDY? + +/* SPI chip selects */ + +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + +/* User GPIOs + * + * GPIO0-1 are the buffered high-power GPIOs. + * GPIO2-5 are the USART2 pins. + * GPIO6-7 are the CAN2 pins. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) +#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* 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) + +/* PWM + * + * The PX4FMU has five PWM outputs, of which only the output on + * pin PC8 is fixed assigned to this function. The other four possible + * pwm sources are the TX, RX, RTS and CTS pins of USART2 + * + * Alternate function mapping: + * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 + */ + +#ifdef CONFIG_PWM +# if defined(CONFIG_STM32_TIM3_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 3 +# elif defined(CONFIG_STM32_TIM8_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 8 +# endif +#endif + +/**************************************************************************************************** + * 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 diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..cb8918306b --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c new file mode 100644 index 0000000000..b5d00eac0d --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +#include +#include + +#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) +{ + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL); + stm32_configgpio(GPIO_SPI_CS_MPU); + stm32_configgpio(GPIO_SPI_CS_SDCARD); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); +} + +__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_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + break; + + case PX4_SPIDEV_ACCEL: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +__EXPORT void stm32_spi2select(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) { + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +__EXPORT void stm32_spi3select(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_SDCARD, !selected); +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ + return SPI_STATUS_PRESENT; +} + diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c new file mode 100644 index 0000000000..b0b669fbed --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -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 + +#include +#include +#include +#include + +#include +#include + +#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); +} + From 76497502cb696a1c6b9b01ed8ef5c3a5a740cb52 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:20:31 -0700 Subject: [PATCH 2/3] Moved the L3GD20 driver to the new driver, working on FMU v1 and v2 --- apps/drivers/l3gd20/Makefile | 42 ------------------------- makefiles/config_px4fmuv2_default.mk | 2 +- nuttx/configs/px4fmuv2/nsh/appconfig | 2 -- {apps => src}/drivers/l3gd20/l3gd20.cpp | 0 src/drivers/l3gd20/module.mk | 6 ++++ 5 files changed, 7 insertions(+), 45 deletions(-) delete mode 100644 apps/drivers/l3gd20/Makefile rename {apps => src}/drivers/l3gd20/l3gd20.cpp (100%) create mode 100644 src/drivers/l3gd20/module.mk diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile deleted file mode 100644 index 98e2d57c55..0000000000 --- a/apps/drivers/l3gd20/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Makefile to build the L3GD20 driver. -# - -APPNAME = l3gd20 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d296c5379c..ee0c17bcb4 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -12,6 +12,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # MODULES += drivers/boards/px4fmuv2 MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 MODULES += drivers/px4fmu MODULES += drivers/rgbled @@ -43,7 +44,6 @@ BUILTIN_COMMANDS := \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fff1406733..205b4448c7 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -115,8 +115,6 @@ CONFIGURED_APPS += drivers/boards/px4fmuv2 CONFIGURED_APPS += drivers/device # XXX needs conversion to SPI #CONFIGURED_APPS += drivers/ms5611 -CONFIGURED_APPS += drivers/l3gd20 -CONFIGURED_APPS += drivers/lsm303d # XXX needs conversion to serial #CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp similarity index 100% rename from apps/drivers/l3gd20/l3gd20.cpp rename to src/drivers/l3gd20/l3gd20.cpp diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk new file mode 100644 index 0000000000..23e77e8712 --- /dev/null +++ b/src/drivers/l3gd20/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = l3gd20 +SRCS = l3gd20.cpp From b7e947cb3d53cbb0f9b194980a6a63588ba56bf2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:22:08 -0700 Subject: [PATCH 3/3] Anti-Aliasing frequency of the LSM303D can now be read too, not just written --- src/drivers/lsm303d/lsm303d.cpp | 52 ++++++++++++++++++++++++++++++--- 1 file changed, 48 insertions(+), 4 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8a64ee7024..ba7316e557 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -321,7 +321,15 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned max_g); + int set_antialias_filter_bandwidth(unsigned bandwith); + + /** + * Get the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * @return OK if the value was read and supported, ERROR otherwise. + */ + int get_antialias_filter_bandwidth(unsigned &bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -477,10 +485,10 @@ LSM303D::init() /* XXX should we enable FIFO? */ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */ + set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ - mag_set_range(12); /* XXX: take highest sensor range of 12GA? */ + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); /* XXX test this when another mag is used */ @@ -687,6 +695,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; +// case ACCELIOCSLOWPASS: + case ACCELIOCGLOWPASS: + + unsigned bandwidth; + + if (OK == get_antialias_filter_bandwidth(bandwidth)) + return bandwidth; + else + return -EINVAL; + + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -942,6 +961,25 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) +{ + uint8_t readbits = read_reg(ADDR_CTRL_REG2); + + if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) + bandwidth = 50; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) + bandwidth = 194; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) + bandwidth = 362; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) + bandwidth = 773; + else + return ERROR; + + return OK; +} + int LSM303D::set_samplerate(unsigned frequency) { @@ -1305,6 +1343,7 @@ test() int fd_accel = -1; struct accel_report a_report; ssize_t sz; + int filter_bandwidth; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1318,14 +1357,19 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + warnx("accel antialias filter bandwidth: fail"); + else + warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); int fd_mag = -1; struct mag_report m_report;