diff options
Diffstat (limited to 'src')
76 files changed, 7045 insertions, 889 deletions
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..b4c5fa06e 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,9 +92,6 @@ #include <nuttx/config.h> -#include <arch/board/board.h> -#include <drivers/device/i2c.h> - #include <sys/types.h> #include <stdint.h> #include <string.h> @@ -104,16 +101,19 @@ #include <unistd.h> #include <stdio.h> #include <ctype.h> - -#include <drivers/drv_blinkm.h> +#include <poll.h> #include <nuttx/wqueue.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> - #include <systemlib/systemlib.h> -#include <poll.h> + +#include <board_config.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_blinkm.h> + #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_gps_position.h> @@ -486,15 +486,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { + + for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } - if(new_data_vehicle_status || no_data_vehicle_status < 3){ - if(num_of_cells == 0) { + if (new_data_vehicle_status || no_data_vehicle_status < 3) { + if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { @@ -831,6 +831,8 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); fprintf(stderr, "options:\n"); diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 4409a8a9c..cfb625670 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -59,7 +59,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/device/spi.h> #include <drivers/drv_accel.h> diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h index 383a046ff..9d7c81f85 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_internal.h + * @file board_config.h * * PX4FMU internal definitions */ @@ -51,13 +51,16 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include <stm32.h> - +#include <arch/board/board.h> /**************************************************************************************************** * Definitions ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" + //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" //#endif @@ -74,15 +77,45 @@ __BEGIN_DECLS /* 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) +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL 2 +#define PX4_SPIDEV_MPU 3 + +/* + * Optional devices on IO's external port + */ +#define PX4_SPIDEV_ACCEL_MAG 2 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_MS5611 0x76 +#define PX4_I2C_OBDEV_EEPROM NOTDEFINED + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x1a + /* User GPIOs * * GPIO0-1 are the buffered high-power GPIOs. @@ -107,31 +140,45 @@ __BEGIN_DECLS #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) +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/* + * PWM + * + * Four PWM outputs can be configured on pins otherwise shared with + * USART2; two can take the flow control pins if they are not being used. + * + * Pins: + * + * CTS - PA0 - TIM2CH1 + * RTS - PA1 - TIM2CH2 + * TX - PA2 - TIM2CH3 + * RX - PA3 - TIM2CH4 + * + */ +#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1 +#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 +#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 +#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 + /* 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 +/* High-resolution timer */ - -#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 +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b776917..66b776917 100644 --- a/src/drivers/boards/px4fmu/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c index 187689ff9..1e1f10040 100644 --- a/src/drivers/boards/px4fmu/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c @@ -54,7 +54,7 @@ #include "stm32.h" #include "stm32_can.h" -#include "px4fmu_internal.h" +#include "board_config.h" #ifdef CONFIG_CAN diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 36af2298c..964f5069c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -59,7 +59,7 @@ #include <nuttx/analog/adc.h> #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include <arch/board/board.h> diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 31b25984e..aa83ec130 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -42,7 +42,7 @@ #include <stdbool.h> #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include <arch/board/board.h> diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c index d85131dd8..848e21d79 100644 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c @@ -41,15 +41,15 @@ #include <stdint.h> -#include <drivers/stm32/drv_pwm_servo.h> - -#include <arch/board/board.h> -#include <drivers/drv_pwm_output.h> - #include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> +#include <drivers/stm32/drv_pwm_servo.h> +#include <drivers/drv_pwm_output.h> + +#include "board_config.h" + __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM2_BASE, diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index e05ddecf3..17e6862f7 100644 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "chip.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Public Functions diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c index 0be981c1e..0fc8569aa 100644 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h new file mode 100644 index 000000000..ec8dde499 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 board_config.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.h> +#include <arch/board/board.h> + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) + +/* 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) + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* 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_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) +#define GPIO_VDD_3V3_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) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* 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) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/**************************************************************************************************** + * 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-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk new file mode 100644 index 000000000..99d37eeca --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -0,0 +1,10 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu2_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c new file mode 100644 index 000000000..135767b26 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -0,0 +1,262 @@ +/**************************************************************************** + * + * 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/sdio.h> +#include <nuttx/mmcsd.h> +#include <nuttx/analog/adc.h> + +#include <stm32.h> +#include "board_config.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; +static struct sdio_dev_s *sdio; + +#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_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); + 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(); + led_off(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\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\n"); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + + message("[boot] Successfully initialized SPI port 2\n"); + + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c new file mode 100644 index 000000000..5ded4294e --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> + +#include "stm32.h" +#include "board_config.h" + +#include <arch/board/board.h> + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c new file mode 100644 index 000000000..f66c7cd79 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/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 <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 "board_config.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-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c new file mode 100644 index 000000000..600dfef41 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c @@ -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 <stm32.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#include <drivers/stm32/drv_pwm_servo.h> +#include <drivers/drv_pwm_output.h> + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .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, + } +}; diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c new file mode 100644 index 000000000..09838d02f --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -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.h> +#include "board_config.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 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c new file mode 100644 index 000000000..f329e06ff --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/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 <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.h> +#include "board_config.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); +} + diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h index 6638e715e..48aadbd76 100644 --- a/src/drivers/boards/px4io/px4io_internal.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4io_internal.h + * @file board_config.h * * PX4IO hardware definitions. */ @@ -47,7 +47,9 @@ #include <nuttx/compiler.h> #include <stdint.h> +/* these headers are not C++ safe */ #include <stm32.h> +#include <arch/board/board.h> /************************************************************************************ * Definitions @@ -83,3 +85,11 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15..2601a1c15 100644 --- a/src/drivers/boards/px4io/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 15c59e423..8292da9e1 100644 --- a/src/drivers/boards/px4io/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -55,7 +55,7 @@ #include <nuttx/arch.h> #include "stm32.h" -#include "px4io_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include <arch/board/board.h> diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c index 6df470da6..6df470da6 100644 --- a/src/drivers/boards/px4io/px4io_pwm_servo.c +++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h new file mode 100644 index 000000000..818b64436 --- /dev/null +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> +#include <stdint.h> + +/* these headers are not C++ safe */ +#include <stm32.h> +#include <arch/board/board.h> + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * Serial + ******************************************************************************/ +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) + +/* Safety switch button *******************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) + +/* Analog inputs **************************************************************/ + +#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) + +/* the same rssi signal goes to both an adc and a timer input */ +#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) +#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +/* XXX these should be UART pins */ +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk new file mode 100644 index 000000000..85f94e8be --- /dev/null +++ b/src/drivers/boards/px4io-v2/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c \ + px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c new file mode 100644 index 000000000..0ea95bded --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -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 px4iov2_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 <stm32.h> +#include "board_config.h" + +#include <arch/board/board.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 GPIOs */ + + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - disable it by default */ + /* XXX might not want to do this on warm restart? */ + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + + /* RSSI inputs */ + stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + stm32_configgpio(GPIO_ADC_RSSI); + + /* servo rail voltage */ + stm32_configgpio(GPIO_ADC_VSERVO); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); + stm32_configgpio(GPIO_SBUS_OUTPUT); + + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(GPIO_SBUS_OENABLE, true); + stm32_configgpio(GPIO_SBUS_OENABLE); + + stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_PWM1, false); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, false); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, false); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, false); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, false); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, false); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, false); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, false); + stm32_configgpio(GPIO_PWM8); +} diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c new file mode 100644 index 000000000..4f1b9487c --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4iov2_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.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +__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 + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_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_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_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 510983d4b..37af26d52 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -59,17 +59,47 @@ # define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ /** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifndef PX4IO_DEVICE_PATH -# error No GPIO support for this board. +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/* + * PX4FMUv2 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - !VDD_5V_PERIPH_EN */ +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ +# define GPIO_SERVO_VALID (1<<9) /**< PB7 - !VDD_SERVO_VALID */ +# define GPIO_5V_HIPOWER_OC (1<<10) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" + +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 +/* no GPIO driver on the PX4IOv1 board */ +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +/* no GPIO driver on the PX4IOv2 board */ #endif /* diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 21044f620..97f2db107 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -60,6 +60,6 @@ __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(void); +__EXPORT void drv_led_start(void); __END_DECLS diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index cd72d9d23..257b41935 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -59,7 +59,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/airspeed.h> #include <systemlib/err.h> diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ac3bdc132..039b496f4 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -58,7 +58,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1ffca2f43..05739f04f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -59,11 +59,11 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <arch/board/board.h> - #include <drivers/device/spi.h> #include <drivers/drv_gyro.h> +#include <board_config.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -78,6 +84,7 @@ static const int ERROR = -1; /* register addresses */ #define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 #define ADDR_CTRL_REG1 0x20 @@ -85,8 +92,8 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -185,9 +192,14 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -278,7 +290,11 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) + _orientation(SENSOR_BOARD_ROTATION_270_DEG), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(250, 30), + _gyro_filter_y(250, 30), + _gyro_filter_z(250, 30) { // enable debug() calls _debug_enabled = true; @@ -333,7 +349,6 @@ L3GD20::init() write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we @@ -341,7 +356,7 @@ L3GD20::init() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_range(500); /* default to 500dps */ + set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ ret = OK; @@ -355,9 +370,24 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; + return OK; + } return -EIO; } @@ -446,6 +476,13 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; + // adjust filters + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -498,10 +535,17 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGSAMPLERATE: return _current_rate; - case GYROIOCSLOWPASS: + case GYROIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -563,28 +607,32 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; @@ -598,19 +646,20 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - if (frequency <= 95) { + // use limits good for H or non-H models + if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 190) { + } else if (frequency <= 200) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; - } else if (frequency <= 380) { + } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_20HZ; - } else if (frequency <= 760) { + } else if (frequency <= 800) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; @@ -691,14 +740,43 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + report->x = _gyro_filter_x.apply(report->x); + report->y = _gyro_filter_y.apply(report->y); + report->z = _gyro_filter_z.apply(report->z); + report->scaling = _gyro_range_scale; report->range_rad_s = _gyro_range_rad_s; @@ -749,7 +827,7 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 04b565358..fe307223f 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 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 @@ -110,11 +110,11 @@ LED *gLED; } void -drv_led_start() +drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; if (gLED != nullptr) gLED->init(); } -}
\ No newline at end of file +} diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp new file mode 100644 index 000000000..117583faf --- /dev/null +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -0,0 +1,1549 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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 lsm303d.cpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stddef.h> +#include <stdlib.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <nuttx/arch.h> +#include <nuttx/clock.h> + +#include <drivers/drv_hrt.h> +#include <drivers/device/spi.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_mag.h> + +#include <board_config.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG0 0x1F +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG6 0x25 +#define ADDR_CTRL_REG7 0x26 + +#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) + +#define REG1_BDU_UPDATE (1<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) + +#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) + +#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) + +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) + +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) + + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + + +class LSM303D_mag; + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + LSM303D_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + unsigned _current_samplerate; + + unsigned _num_mag_reports; + volatile unsigned _next_mag_report; + volatile unsigned _oldest_mag_report; + struct mag_report *_mag_reports; + + struct mag_scale _mag_scale; + float _mag_range_scale; + float _mag_range_ga; + orb_advert_t _mag_topic; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void mag_measure(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); + + /** + * Set the LSM303D mag measurement range. + * + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + 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. + * + * @param frequency The internal accel sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); + + /** + * Set the LSM303D internal mag sampling frequency. + * + * @param frequency The internal mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int mag_set_samplerate(unsigned frequency); +}; + +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _current_samplerate(0), + _num_mag_reports(0), + _next_mag_report(0), + _oldest_mag_report(0), + _mag_reports(nullptr), + _mag_range_scale(0.0f), + _mag_range_ga(0.0f), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _accel_filter_x(800, 30), + _accel_filter_y(800, 30), + _accel_filter_z(800, 30) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_mag_reports != nullptr) + delete[] _mag_reports; + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + int mag_ret; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + /* advertise accel topic */ + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + + _num_mag_reports = 2; + _oldest_mag_report = _next_mag_report = 0; + _mag_reports = new struct mag_report[_num_mag_reports]; + + if (_mag_reports == nullptr) + goto out; + + /* advertise mag topic */ + memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + + /* enable accel, XXX do this with an ioctl? */ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + + /* enable mag, XXX do this with an ioctl? */ + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + /* 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(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ + set_samplerate(400); /* max sample rate */ + + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ + mag_set_samplerate(100); + + /* XXX test this when another mag is used */ + /* do CDev init for the mag device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + + ret = OK; +out: + return ret; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_mag_report != _next_mag_report) { + memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); + ret += sizeof(_mag_reports[0]); + INCREMENT(_oldest_mag_report, _num_mag_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_mag_report = _next_mag_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); + ret = sizeof(*_mag_reports); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + /* XXX for vibration tests with 800 Hz */ + return ioctl(filp, SENSORIOCSPOLLRATE, 800); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* adjust sample rate of sensor */ + set_samplerate(arg); + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_accel_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + case ACCELIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_accel_interval; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 50 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* adjust sample rate of sensor */ + mag_set_samplerate(arg); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct mag_report *buf = new struct mag_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _mag_reports; + _num_mag_reports = arg; + _mag_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_mag_reports - 1; + + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case MAGIOCSSAMPLERATE: +// case MAGIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case MAGIOCSLOWPASS: +// case MAGIOCGLOWPASS: + /* XXX not implemented */ +// _set_dlpf_filter((uint16_t)arg); + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: +// case MAGIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _mag_range_scale = xx + // _mag_range_ga = xx + return -EINVAL; + + case MAGIOCSELFTEST: + /* XXX not implemented */ +// return self_test(); + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::set_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_range_g = 0.0f; + float new_scale_g_digit = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + new_range_g = 2.0f; + setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; + + } else if (max_g <= 4) { + new_range_g = 4.0f; + setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; + + } else if (max_g <= 6) { + new_range_g = 6.0f; + setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; + + } else if (max_g <= 8) { + new_range_g = 8.0f; + setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; + + } else if (max_g <= 16) { + new_range_g = 16.0f; + setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; + + } else { + return -EINVAL; + } + + _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_scale = new_scale_g_digit * 9.80665f; + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_range(unsigned max_ga) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG6_FULL_SCALE_BITS_M; + float new_range = 0.0f; + float new_scale_ga_digit = 0.0f; + + if (max_ga == 0) + max_ga = 12; + + if (max_ga <= 2) { + new_range = 2.0f; + setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; + + } else if (max_ga <= 4) { + new_range = 4.0f; + setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; + + } else if (max_ga <= 8) { + new_range = 8.0f; + setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; + + } else if (max_ga <= 12) { + new_range = 12.0f; + setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; + + } else { + return -EINVAL; + } + + _mag_range_ga = new_range; + _mag_range_scale = new_scale_ga_digit; + + modify_reg(ADDR_CTRL_REG6, clearbits, setbits); + + return OK; +} + +int +LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + 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) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + _current_samplerate = 100; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + _current_samplerate = 200; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + _current_samplerate = 400; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + _current_samplerate = 800; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + _current_samplerate = 1600; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG5_RATE_BITS_M; + + if (frequency == 0) + frequency = 100; + + if (frequency <= 25) { + setbits |= REG5_RATE_25HZ_M; + + } else if (frequency <= 50) { + setbits |= REG5_RATE_50HZ_M; + + } else if (frequency <= 100) { + setbits |= REG5_RATE_100HZ_M; + + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG5, clearbits, setbits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_accel_report = _next_accel_report = 0; + _oldest_mag_report = _next_mag_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +LSM303D::measure() +{ + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; +#pragma pack(pop) + + accel_report *accel_report = &_accel_reports[_next_accel_report]; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + /* fetch data from the sensor */ + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + accel_report->timestamp = hrt_absolute_time(); + + accel_report->x_raw = raw_accel_report.x; + accel_report->y_raw = raw_accel_report.y; + accel_report->z_raw = raw_accel_report.z; + + float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + accel_report->x = _accel_filter_x.apply(x_in_new); + accel_report->y = _accel_filter_y.apply(y_in_new); + accel_report->z = _accel_filter_z.apply(z_in_new); + + accel_report->scaling = _accel_range_scale; + accel_report->range_m_s2 = _accel_range_m_s2; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report *mag_report = &_mag_reports[_next_mag_report]; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report->timestamp = hrt_absolute_time(); + + mag_report->x_raw = raw_mag_report.x; + mag_report->y_raw = raw_mag_report.y; + mag_report->z_raw = raw_mag_report.z; + mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report->scaling = _mag_range_scale; + mag_report->range_ga = _mag_range_ga; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_mag_report, _num_mag_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_mag_report == _oldest_mag_report) + INCREMENT(_oldest_mag_report, _num_mag_reports); + + /* XXX please check this poll_notify, is it the right one? */ + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +void +LSM303D::print_info() +{ + perf_print_counter(_accel_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); + perf_print_counter(_mag_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd, fd_mag; + + if (g_dev != nullptr) + errx(0, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report accel_report; + ssize_t sz; + int filter_bandwidth; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_accel, &accel_report, sizeof(accel_report)); + + if (sz != sizeof(accel_report)) + err(1, "immediate read failed"); + + + warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); + warnx("accel x: \t%d\traw", (int)accel_report.x_raw); + warnx("accel y: \t%d\traw", (int)accel_report.y_raw); + warnx("accel z: \t%d\traw", (int)accel_report.z_raw); + + warnx("accel range: %8.4f m/s^2", (double)accel_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; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + warnx("mag x: \t% 9.5f\tga", (double)m_report.x); + warnx("mag y: \t% 9.5f\tga", (double)m_report.y); + warnx("mag z: \t% 9.5f\tga", (double)m_report.z); + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + warnx("mag range: %8.4f ga", (double)m_report.range_ga); + + /* XXX add poll-rate tests here too */ + +// reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + lsm303d::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk new file mode 100644 index 000000000..e40f718c5 --- /dev/null +++ b/src/drivers/lsm303d/module.mk @@ -0,0 +1,8 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = lsm303d +SRCS = lsm303d.cpp + + diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 397686e8b..c5f49fb36 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -59,8 +59,6 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> - #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -70,6 +68,8 @@ #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> +#include <board_config.h> + /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 68d2c5d65..666bd30e6 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -70,7 +70,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/airspeed.h> #include <systemlib/err.h> diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index e78d96569..4ad13fc04 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -59,10 +59,11 @@ #include <nuttx/arch.h> #include <nuttx/i2c.h> +#include <board_config.h> + #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -#include <drivers/boards/px4fmu/px4fmu_internal.h> #include <drivers/drv_hrt.h> #include <drivers/drv_rc_input.h> @@ -1345,44 +1346,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, gpio_bits = 0; servo_mode = MK::MODE_NONE; - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = MK::MODE_4PWM; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - break; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c4e331a30..65fa33530 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -63,7 +63,7 @@ #include <nuttx/arch.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include <drivers/device/spi.h> diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 3c4b0f093..20f8aa173 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp +SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index c13b65d5a..d9268c0b3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 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 @@ -33,13 +33,11 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ #include <nuttx/config.h> -#include <drivers/device/i2c.h> - #include <sys/types.h> #include <stdint.h> #include <stdbool.h> @@ -59,12 +57,14 @@ #include <arch/board/board.h> +#include <drivers/device/device.h> +#include <drivers/drv_baro.h> #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> -#include <drivers/drv_baro.h> +#include "ms5611.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -76,35 +76,25 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) -/** - * Grody hack for crc4() +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * MS5611 internal constants and data structures. */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) -class MS5611 : public device::I2C +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +class MS5611 : public device::CDev { public: - MS5611(int bus); - virtual ~MS5611(); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf); + ~MS5611(); virtual int init(); @@ -117,10 +107,9 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; -private: - union ms5611_prom_u _prom; + ms5611::prom_s _prom; struct work_s _work; unsigned _measure_ticks; @@ -149,16 +138,7 @@ private: perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. + * Initialize the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. @@ -198,70 +178,23 @@ private: * * @return OK if the measurement command was successful. */ - int measure(); + virtual int measure(); /** * Collect the result of the most recent measurement. */ - int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - int read_prom(); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - + virtual int collect(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ - -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ - -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ - /* * Driver 'main' command. */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : + CDev("MS5611", BARO_DEVICE_PATH), + _interface(interface), + _prom(prom_buf.s), _measure_ticks(0), _num_reports(0), _next_report(0), @@ -279,10 +212,7 @@ MS5611::MS5611(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -294,23 +224,30 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + delete _interface; } int MS5611::init() { - int ret = ERROR; + int ret; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; - if (_reports == nullptr) + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; goto out; + } _oldest_report = _next_report = 0; @@ -318,49 +255,17 @@ MS5611::init() memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - if (_baro_topic < 0) + if (_baro_topic < 0) { debug("failed to create sensor_baro object"); + ret = -ENOSPC; + goto out; + } ret = OK; out: return ret; } -int -MS5611::probe() -{ - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } - - return -EIO; -} - -int -MS5611::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { @@ -546,8 +451,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) break; } - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } void @@ -572,7 +478,7 @@ MS5611::stop_cycle() void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast<MS5611 *>(arg); dev->cycle(); } @@ -592,7 +498,7 @@ MS5611::cycle() /* * The ms5611 seems to regularly fail to respond to * its address; this happens often enough that we'd rather not - * spam the console with the message. + * spam the console with a message for this. */ } else { //log("collection error %d", ret); @@ -654,17 +560,12 @@ MS5611::measure() /* * In phase zero, request temperature; in other phases, request pressure. */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); - + ret = _interface->ioctl(IOCTL_MEASURE, addr); if (OK != ret) perf_count(_comms_errors); @@ -677,47 +578,33 @@ int MS5611::collect() { int ret; - uint8_t cmd; - uint8_t data[3]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - cmd = 0; + uint32_t raw; perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - ret = transfer(&cmd, 1, &data[0], 3); - if (ret != OK) { + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - /* handle a measurement */ if (_measure_phase == 0) { /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); /* temperature compensation */ if (_TEMP < 2000) { @@ -761,30 +648,7 @@ MS5611::collect() * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ @@ -807,7 +671,7 @@ MS5611::collect() * a */ _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif + /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); @@ -832,55 +696,49 @@ MS5611::collect() return OK; } -int -MS5611::cmd_reset() +void +MS5611::print_info() { - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - return result; + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); } -int -MS5611::read_prom() +/** + * Local functions in support of the shell command. + */ +namespace ms5611 { - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; - - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } +MS5611 *g_dev; - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} +void start(); +void test(); +void reset(); +void info(); +void calibrate(unsigned altitude); +/** + * MS5611 crc4 cribbed from the datasheet + */ bool -MS5611::crc4(uint16_t *n_prom) +crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -922,43 +780,6 @@ MS5611::crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } -void -MS5611::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", _SENS); - printf("OFF: %lld\n", _OFF); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -MS5611 *g_dev; - -void start(); -void test(); -void reset(); -void info(); -void calibrate(unsigned altitude); /** * Start the driver. @@ -967,28 +788,46 @@ void start() { int fd; + prom_u prom_buf; if (g_dev != nullptr) /* if already started, the still command succeeded */ errx(0, "already started"); - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); + device::Device *interface = nullptr; - if (g_dev == nullptr) - goto fail; + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ + if (MS5611_spi_interface != nullptr) + interface = MS5611_spi_interface(prom_buf); + if (interface == nullptr && (MS5611_i2c_interface != nullptr)) + interface = MS5611_i2c_interface(prom_buf); - if (OK != g_dev->init()) + if (interface == nullptr) + errx(1, "failed to allocate an interface"); + + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + g_dev = new MS5611(interface, prom_buf); + if (g_dev == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev->init() != OK) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) + if (fd < 0) { + warnx("can't open baro device"); goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("failed setting default poll rate"); goto fail; + } exit(0); @@ -1155,11 +994,11 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", p1); + warnx("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h new file mode 100644 index 000000000..76fb84de8 --- /dev/null +++ b/src/drivers/ms5611/ms5611.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 ms5611.h + * + * Shared defines for the ms5611 driver. + */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +namespace ms5611 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +extern bool crc4(uint16_t *n_prom); + +} /* namespace */ + +/* interface factories */ +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; + diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp new file mode 100644 index 000000000..87d9b94a6 --- /dev/null +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 ms5611_i2c.cpp + * + * I2C interface for MS5611 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/i2c.h> + +#include "ms5611.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_MS5611 + +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + + + +device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); + +class MS5611_I2C : public device::I2C +{ +public: + MS5611_I2C(int bus, ms5611::prom_u &prom_buf); + virtual ~MS5611_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u &_prom; + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_i2c_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_I2C(MS5611_BUS, prom_buf); +} + +MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : + I2C("MS5611_I2C", nullptr, bus, 0, 400000), + _prom(prom) +{ +} + +MS5611_I2C::~MS5611_I2C() +{ +} + +int +MS5611_I2C::init() +{ + /* this will call probe(), and thereby _probe_address */ + return I2C::init(); +} + +int +MS5611_I2C::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_I2C::probe() +{ + _retries = 10; + + if ((OK == _probe_address(MS5611_ADDRESS_1)) || + (OK == _probe_address(MS5611_ADDRESS_2))) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return OK; + } + + return -EIO; +} + +int +MS5611_I2C::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != _reset()) + return -EIO; + + /* read PROM */ + if (OK != _read_prom()) + return -EIO; + + return OK; +} + + +int +MS5611_I2C::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_I2C::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_I2C::_read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp new file mode 100644 index 000000000..f6c624340 --- /dev/null +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -0,0 +1,273 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 ms5611_spi.cpp + * + * SPI interface for MS5611 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/spi.h> + +#include "ms5611.h" +#include "board_config.h" + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#ifdef PX4_SPIDEV_BARO + +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); + +class MS5611_SPI : public device::SPI +{ +public: + MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); + virtual ~MS5611_SPI(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + ms5611::prom_u &_prom; + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + + /** + * Read a 16-bit register value. + * + * @param reg The register to read. + */ + uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); +}; + +device::Device * +MS5611_spi_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +} + +MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + _prom(prom_buf) +{ +} + +MS5611_SPI::~MS5611_SPI() +{ +} + +int +MS5611_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + goto out; + } + + /* send reset command */ + ret = _reset(); + if (ret != OK) { + debug("reset failed"); + goto out; + } + + /* read PROM */ + ret = _read_prom(); + if (ret != OK) { + debug("prom readout failed"); + goto out; + } + +out: + return ret; +} + +int +MS5611_SPI::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[4]; + + /* read the most recent measurement */ + buf[0] = 0 | DIR_WRITE; + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); + + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[3]; + cvt->b[1] = buf[2]; + cvt->b[2] = buf[1]; + cvt->b[3] = 0; + + ret = count; + } + + return ret; +} + +int +MS5611_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + if (ret != OK) { + errno = ret; + return -1; + } + return 0; +} + +int +MS5611_SPI::_reset() +{ + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; + + return _transfer(&cmd, nullptr, 1); +} + +int +MS5611_SPI::_measure(unsigned addr) +{ + uint8_t cmd = addr | DIR_WRITE; + + return _transfer(&cmd, nullptr, 1); +} + + +int +MS5611_SPI::_read_prom() +{ + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = _reg16(cmd); + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +uint16_t +MS5611_SPI::_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + _transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + irqstate_t flags = irqsave(); + + int ret = transfer(send, recv, len); + + irqrestore(flags); + + return ret; +} + +#endif /* PX4_SPIDEV_BARO */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a98b527b1..a6f337486 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -34,7 +34,7 @@ /** * @file fmu.cpp * - * Driver/configurator for the PX4 FMU multi-purpose port. + * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ #include <nuttx/config.h> @@ -57,9 +57,10 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -#include <drivers/boards/px4fmu/px4fmu_internal.h> #include <drivers/drv_hrt.h> +# include <board_config.h> + #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> @@ -71,15 +72,18 @@ #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> -#include <systemlib/ppm_decode.h> +#ifdef HRT_PPM_CHANNEL +# include <systemlib/ppm_decode.h> +#endif class PX4FMU : public device::CDev { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -146,6 +150,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -154,6 +159,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -271,9 +292,36 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: // multi-port with flow control lines as PWM - case MODE_4PWM: // multi-port as 4 PWM outs - debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + debug("MODE_2PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_4PWM: // v1 multi-port as 4 PWM outs + debug("MODE_4PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_6PWM: // v2 PWMs as 6 PWM outs + debug("MODE_6PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -281,7 +329,7 @@ PX4FMU::set_mode(Mode mode) _pwm_alt_rate_channels = 0; /* XXX magic numbers */ - up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + up_pwm_servo_init(0x3f); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -399,14 +447,14 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; - +#ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; +#endif log("starting"); @@ -460,6 +508,23 @@ PX4FMU::task_main() /* can we mix? */ if (_mixers != nullptr) { + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; + } + /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.timestamp = hrt_absolute_time(); @@ -512,6 +577,7 @@ PX4FMU::task_main() } } +#ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. @@ -531,6 +597,8 @@ PX4FMU::task_main() orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } +#endif + } ::close(_t_actuators); @@ -579,6 +647,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -623,16 +692,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; - case PWM_SERVO_SET(2): + case PWM_SERVO_SET(5): + case PWM_SERVO_SET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_SET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): + case PWM_SERVO_SET(0): if (arg < 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { @@ -641,16 +718,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); break; @@ -658,16 +743,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): + case PWM_SERVO_GET_RATEGROUP(4): + case PWM_SERVO_GET_RATEGROUP(5): *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { + switch (_mode) { + case MODE_6PWM: + *(unsigned *)arg = 6; + break; + case MODE_4PWM: *(unsigned *)arg = 4; - - } else { + break; + case MODE_2PWM: *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; } break; @@ -743,17 +838,17 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[4]; + uint16_t values[6]; - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + if (count > 6) { + // we have at most 6 outputs + count = 6; } // allow for misaligned values - memcpy(values, buffer, count*2); + memcpy(values, buffer, count * 2); - for (uint8_t i=0; i<count; i++) { + for (uint8_t i = 0; i < count; i++) { up_pwm_servo_set(i, values[i]); } return count * 2; @@ -763,19 +858,28 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); +#endif } void PX4FMU::gpio_set_function(uint32_t gpios, int function) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -787,6 +891,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) if (GPIO_SET_OUTPUT == function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } +#endif /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { @@ -809,9 +914,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); +#endif } void @@ -912,14 +1019,21 @@ fmu_new_mode(PortMode new_mode) /* nothing more to do here */ break; - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - case PORT_FULL_PWM: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + servo_mode = PX4FMU::MODE_6PWM; +#endif + break; + + /* mixed modes supported on v1 board only */ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; break; case PORT_GPIO_AND_SERIAL: @@ -938,6 +1052,9 @@ fmu_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = PX4FMU::MODE_2PWM; break; +#endif + default: + return -1; } /* adjust GPIO config for serial mode(s) */ @@ -979,22 +1096,84 @@ void test(void) { int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } + + warnx("Testing %u servos", (unsigned)servo_count); + + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + for (;;) { + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i=0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + } else { + // and use write interface for the other direction + int ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + break; + } + } + } + close(console); close(fd); exit(0); @@ -1053,12 +1232,13 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "mode_gpio")) { new_mode = PORT_FULL_GPIO; - } else if (!strcmp(verb, "mode_serial")) { - new_mode = PORT_FULL_SERIAL; - } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(verb, "mode_gpio_serial")) { new_mode = PORT_GPIO_AND_SERIAL; @@ -1067,6 +1247,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm_gpio")) { new_mode = PORT_PWM_AND_GPIO; +#endif } /* was a new mode set? */ @@ -1088,6 +1269,10 @@ fmu_main(int argc, char *argv[]) fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + fprintf(stderr, " mode_gpio, mode_pwm, test\n"); +#endif exit(1); } diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 328e5a684..2054faa12 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,4 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp + px4io_uploader.cpp \ + px4io_serial.cpp \ + px4io_i2c.cpp + +# XXX prune to just get UART registers +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fed83ea1a..cebe33996 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -58,7 +58,6 @@ #include <arch/board/board.h> #include <drivers/device/device.h> -#include <drivers/device/i2c.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> @@ -83,9 +82,13 @@ #include <debug.h> #include <mavlink/mavlink_log.h> -#include "uploader.h" #include <modules/px4iofirmware/protocol.h> +#include "uploader.h" + +extern device::Device *PX4IO_i2c_interface() weak_function; +extern device::Device *PX4IO_serial_interface() weak_function; + #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) @@ -94,7 +97,7 @@ * * Encapsulates PX4FMU to PX4IO communications modeled as file operations. */ -class PX4IO : public device::I2C +class PX4IO : public device::CDev { public: /** @@ -102,7 +105,8 @@ public: * * Initialize all class variables. */ - PX4IO(); + PX4IO(device::Device *interface); + /** * Destructor. * @@ -143,8 +147,8 @@ public: /** * Set the update rate for actuator outputs from FMU to IO. * - * @param[in] rate The rate in Hz actuator output are sent to IO. - * Min 10 Hz, max 400 Hz + * @param[in] rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -159,8 +163,8 @@ public: /** * Push failsafe values to IO. * - * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) - * @param[in] len Number of channels, could up to 8 + * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param[in] len Number of channels, could up to 8 */ int set_failsafe_values(const uint16_t *vals, unsigned len); @@ -192,7 +196,10 @@ public: }; private: + device::Device *_interface; + // XXX + unsigned _hardware; ///< Hardware revision unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO unsigned _max_controls; ///<Maximum # of controls supported by PX4IO unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO @@ -298,7 +305,7 @@ private: * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. - * @return Zero if all values were successfully written. + * @return OK if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -308,7 +315,7 @@ private: * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. - * @return Zero if the value was written successfully. + * @return OK if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); @@ -319,7 +326,7 @@ private: * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. - * @return Zero if all values were successfully read. + * @return OK if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); @@ -376,8 +383,10 @@ PX4IO *g_dev; } -PX4IO::PX4IO() : - I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), +PX4IO::PX4IO(device::Device *interface) : + CDev("px4io", PX4IO_DEVICE_PATH), + _interface(interface), + _hardware(0), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -429,6 +438,9 @@ PX4IO::~PX4IO() if (_task != -1) task_delete(_task); + if (_interface != nullptr) + delete _interface; + g_dev = nullptr; } @@ -440,31 +452,30 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = I2C::init(); + ret = CDev::init(); if (ret != OK) return ret; - /* - * Enable a couple of retries for operations to IO. - * - * Register read/write operations are intentionally idempotent - * so this is safe as designed. - */ - _retries = 2; - /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); - _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || - (_max_relays < 1) || (_max_relays > 255) || - (_max_transfer < 16) || (_max_transfer > 255) || + (_max_relays > 32) || + (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("failed getting parameters from PX4IO"); - mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + log("config read error"); + mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -596,7 +607,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -640,6 +651,14 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + if ((_t_actuators < 0) || + (_t_armed < 0) || + (_t_vstatus < 0) || + (_t_param < 0)) { + log("subscription(s) failed"); + goto out; + } + /* poll descriptor */ pollfd fds[4]; fds[0].fd = _t_actuators; @@ -755,6 +774,7 @@ PX4IO::task_main() unlock(); +out: debug("exiting"); /* clean up the alternate device node */ @@ -1037,38 +1057,38 @@ int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; - int ret = OK; + int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + /* - * XXX Because the channel count and channel data are fetched - * separately, there is a risk of a race between the two - * that could leave us with channel data and a count that - * are out of sync. - * Fixing this would require a guarantee of atomicity from - * IO, and a single fetch for both count and channels. - * - * XXX Since IO has the input calibration info, we ought to be - * able to get the pre-fixed-up controls directly. + * Read the channel count and the first 9 channels. * - * XXX can we do this more cheaply? If we knew we had DMA, it would - * almost certainly be better to just get all the inputs... + * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - if (channel_count == _io_reg_get_error) - return -EIO; - if (channel_count > RC_INPUT_MAX_CHANNELS) - channel_count = RC_INPUT_MAX_CHANNELS; - input_rc.channel_count = channel_count; + input_rc.timestamp = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + if (ret != OK) + return ret; - if (channel_count > 0) { - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); - if (ret == OK) - input_rc.timestamp = hrt_absolute_time(); + /* + * Get the channel count any any extra channels. This is no more expensive than reading the + * channel count once. + */ + channel_count = regs[0]; + if (channel_count > 9) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + if (ret != OK) + return ret; } + input_rc.channel_count = channel_count; + memcpy(input_rc.values, ®s[prolog], channel_count * 2); + return ret; } @@ -1198,25 +1218,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_NORESTART; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - /* perform the transfer */ - int ret = transfer(msgv, 2); - if (ret != OK) - debug("io_reg_set: error %d", ret); - return ret; + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + if (ret != num_values) { + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); + return -1; + } + return OK; } int @@ -1228,25 +1235,18 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; + } - /* perform the transfer */ - int ret = transfer(msgv, 2); - if (ret != OK) - debug("io_reg_get: data error %d", ret); - return ret; + int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); + if (ret != num_values) { + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); + return -1; + } + return OK; } uint32_t @@ -1254,7 +1254,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1)) + if (io_reg_get(page, offset, &value, 1) != OK) return _io_reg_get_error; return value; @@ -1267,7 +1267,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); - if (ret) + if (ret != OK) return ret; value &= ~clearbits; value |= setbits; @@ -1339,9 +1339,9 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u software %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", @@ -1371,7 +1371,7 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s%s\n", + printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), @@ -1379,18 +1379,26 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); - printf("vbatt %u ibatt %u vbatt scale %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); - printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", - (double)_battery_amp_per_volt, - (double)_battery_amp_bias, - (double)_battery_mamphour_total); + if (_hardware == 1) { + printf("vbatt mV %u ibatt mV %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); + } else if (_hardware == 2) { + printf("vservo %u mV vservo scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); + printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); + } printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); @@ -1561,9 +1569,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - - *(uint32_t *)arg = value; + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; } @@ -1571,7 +1579,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~PX4IO_RELAY1; + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1579,7 +1587,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1588,7 +1596,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); @@ -1686,8 +1694,8 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 5) { - interval_ms = 5; + if (interval_ms < 3) { + interval_ms = 3; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -1712,14 +1720,47 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { +device::Device * +get_interface() +{ + device::Device *interface = nullptr; + +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* try for a serial interface */ + if (PX4IO_serial_interface != nullptr) + interface = PX4IO_serial_interface(); + if (interface != nullptr) + goto got; +#endif + + /* try for an I2C interface if we haven't got a serial one */ + if (PX4IO_i2c_interface != nullptr) + interface = PX4IO_i2c_interface(); + if (interface != nullptr) + goto got; + + errx(1, "cannot alloc interface"); + +got: + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + return interface; +} + void start(int argc, char *argv[]) { if (g_dev != nullptr) errx(1, "already loaded"); + /* allocate the interface */ + device::Device *interface = get_interface(); + /* create the driver - it will set g_dev */ - (void)new PX4IO(); + (void)new PX4IO(interface); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1871,8 +1912,19 @@ monitor(void) } } +void +if_test(unsigned mode) +{ + device::Device *interface = get_interface(); + + int result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + + errx(0, "test returned %d", result); } +} /* namespace */ + int px4io_main(int argc, char *argv[]) { @@ -1883,28 +1935,87 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); - if (!strcmp(argv[1], "limit")) { + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { + printf("[px4io] loaded, detaching first\n"); + /* stop the driver */ + delete g_dev; + } - if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); - } else { - errx(1, "missing argument (50 - 200 Hz)"); - return 1; - } + PX4IO_Uploader *up; + const char *fn[5]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + + } else { + fn[0] = "/fs/microsd/px4io.bin"; + fn[1] = "/etc/px4io.bin"; + fn[2] = "/fs/microsd/px4io2.bin"; + fn[3] = "/etc/px4io2.bin"; + fn[4] = nullptr; + } + + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + + case -ENOENT: + errx(1, "PX4IO firmware file not found"); + + case -EEXIST: + case -EIO: + errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + + case -EINVAL: + errx(1, "verify failed - retry the update"); + + case -ETIMEDOUT: + errx(1, "timed out waiting for bootloader - power-cycle and try again"); + + default: + errx(1, "unexpected error %d", ret); + } + + return ret; + } + + if (!strcmp(argv[1], "iftest")) { + if (g_dev != nullptr) + errx(1, "can't iftest when started"); + + if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); + } + + /* commands below here require a started driver */ + + if (g_dev == nullptr) + errx(1, "not started"); + + if (!strcmp(argv[1], "limit")) { + + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); + } else { + errx(1, "missing argument (50 - 400 Hz)"); + return 1; } exit(0); } if (!strcmp(argv[1], "current")) { - if (g_dev != nullptr) { - if ((argc > 3)) { - g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); - } else { - errx(1, "missing argument (apm_per_volt, amp_offset)"); - return 1; - } + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; } exit(0); } @@ -1915,70 +2026,53 @@ px4io_main(int argc, char *argv[]) errx(1, "failsafe command needs at least one channel value (ppm)"); } - if (g_dev != nullptr) { + /* set values for first 8 channels, fill unassigned channels with 1500. */ + uint16_t failsafe[8]; - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; + for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; + /* set channel to commandline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + failsafe[i] = atoi(argv[i+2]); + if (failsafe[i] < 800 || failsafe[i] > 2200) { + errx(1, "value out of range of 800 < value < 2200. Aborting."); } + } else { + /* a zero value will result in stopping to output any pulse */ + failsafe[i] = 0; } + } - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); - if (ret != OK) - errx(ret, "failed setting failsafe values"); - } else { - errx(1, "not loaded"); - } + if (ret != OK) + errx(ret, "failed setting failsafe values"); exit(0); } if (!strcmp(argv[1], "recovery")) { - if (g_dev != nullptr) { - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1); - } else { - errx(1, "not loaded"); - } + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); exit(0); } if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } + /* stop the driver */ + delete g_dev; exit(0); } if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } + printf("[px4io] loaded\n"); + g_dev->print_status(); exit(0); } @@ -2005,58 +2099,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - /* note, stop not currently implemented */ - - if (!strcmp(argv[1], "update")) { - - if (g_dev != nullptr) { - printf("[px4io] loaded, detaching first\n"); - /* stop the driver */ - delete g_dev; - } - - PX4IO_Uploader *up; - const char *fn[3]; - - /* work out what we're uploading... */ - if (argc > 2) { - fn[0] = argv[2]; - fn[1] = nullptr; - - } else { - fn[0] = "/fs/microsd/px4io.bin"; - fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; - } - - up = new PX4IO_Uploader; - int ret = up->upload(&fn[0]); - delete up; - - switch (ret) { - case OK: - break; - - case -ENOENT: - errx(1, "PX4IO firmware file not found"); - - case -EEXIST: - case -EIO: - errx(1, "error updating PX4IO - check that bootloader mode is enabled"); - - case -EINVAL: - errx(1, "verify failed - retry the update"); - - case -ETIMEDOUT: - errx(1, "timed out waiting for bootloader - power-cycle and try again"); - - default: - errx(1, "unexpected error %d", ret); - } - - return ret; - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp new file mode 100644 index 000000000..19776c40a --- /dev/null +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 px4io_i2c.cpp + * + * I2C interface for PX4IO + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <board_config.h> + +#include <drivers/device/i2c.h> + +#ifdef PX4_I2C_OBDEV_PX4IO + +device::Device *PX4IO_i2c_interface(); + +class PX4IO_I2C : public device::I2C +{ +public: + PX4IO_I2C(int bus, uint8_t address); + virtual ~PX4IO_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + +}; + +device::Device +*PX4IO_i2c_interface() +{ + return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +} + +PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : + I2C("PX4IO_i2c", nullptr, bus, address, 320000) +{ + _retries = 3; +} + +PX4IO_I2C::~PX4IO_I2C() +{ +} + +int +PX4IO_I2C::init() +{ + int ret; + + ret = I2C::init(); + if (ret != OK) + goto out; + + /* XXX really should do something more here */ + +out: + return 0; +} + +int +PX4IO_I2C::ioctl(unsigned operation, unsigned &arg) +{ + return 0; +} + +int +PX4IO_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = 2 * count; + + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; +} + +int +PX4IO_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = 2 * count; + + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; +} + +#endif /* PX4_I2C_OBDEV_PX4IO */ diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp new file mode 100644 index 000000000..236cb918d --- /dev/null +++ b/src/drivers/px4io/px4io_serial.cpp @@ -0,0 +1,673 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 px4io_serial.cpp + * + * Serial interface for PX4IO + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <errno.h> +#include <string.h> + +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <nuttx/arch.h> +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <debug.h> + +#include <drivers/device/device.h> +#include <drivers/drv_hrt.h> +#include <board_config.h> + +#include <systemlib/perf_counter.h> + +#include <modules/px4iofirmware/protocol.h> + +#ifdef PX4IO_SERIAL_BASE + +device::Device *PX4IO_serial_interface(); + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +class PX4IO_serial : public device::Device +{ +public: + PX4IO_serial(); + virtual ~PX4IO_serial(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory + + DMA_HANDLE _tx_dma; + DMA_HANDLE _rx_dma; + + /** saved DMA status */ + static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values + static const unsigned _dma_status_waiting = 0x00000000; + volatile unsigned _rx_dma_status; + + /** bus-ownership lock */ + sem_t _bus_semaphore; + + /** client-waiting lock/signal */ + sem_t _completion_semaphore; + + /** + * Start the transaction with IO and wait for it to complete. + */ + int _wait_complete(); + + /** + * DMA completion handler. + */ + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_rx_dma_callback(unsigned status); + + /** + * Serial interrupt handler. + */ + static int _interrupt(int vector, void *context); + void _do_interrupt(); + + /** + * Cancel any DMA in progress with an error. + */ + void _abort_dma(); + + /** + * Performance counters. + */ + perf_counter_t _pc_txns; + perf_counter_t _pc_dmasetup; + perf_counter_t _pc_retries; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_dmaerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; + +}; + +IOPacket PX4IO_serial::_dma_buffer; +static PX4IO_serial *g_interface; + +device::Device +*PX4IO_serial_interface() +{ + return new PX4IO_serial(); +} + +PX4IO_serial::PX4IO_serial() : + Device("PX4IO_serial"), + _tx_dma(nullptr), + _rx_dma(nullptr), + _rx_dma_status(_dma_status_inactive), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), + _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), + _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) +{ + g_interface = this; +} + +PX4IO_serial::~PX4IO_serial() +{ + if (_tx_dma != nullptr) { + stm32_dmastop(_tx_dma); + stm32_dmafree(_tx_dma); + } + if (_rx_dma != nullptr) { + stm32_dmastop(_rx_dma); + stm32_dmafree(_rx_dma); + } + + /* reset the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + + /* restore the GPIOs */ + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + + /* and kill our semaphores */ + sem_destroy(&_completion_semaphore); + sem_destroy(&_bus_semaphore); + + perf_free(_pc_txns); + perf_free(_pc_dmasetup); + perf_free(_pc_retries); + perf_free(_pc_timeouts); + perf_free(_pc_crcerrs); + perf_free(_pc_dmaerrs); + perf_free(_pc_protoerrs); + perf_free(_pc_uerrs); + perf_free(_pc_idle); + perf_free(_pc_badidle); + + if (g_interface == this) + g_interface = nullptr; +} + +int +PX4IO_serial::init() +{ + /* allocate DMA */ + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) + return -1; + + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* reset & configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + + /* create semaphores */ + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); + + + /* XXX this could try talking to IO */ + + return 0; +} + +int +PX4IO_serial::ioctl(unsigned operation, unsigned &arg) +{ + + switch (operation) { + + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + lowsyslog("test 0\n"); + + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0x55; + } + return 0; + + case 1: + { + unsigned fails = 0; + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; + + if (count >= 5000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } + } + return 0; + } + case 2: + lowsyslog("test 2\n"); + return 0; + } + default: + break; + } + + return -1; +} + +int +PX4IO_serial::write(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + if (count > PKT_MAX_REGS) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = count | PKT_CODE_WRITE; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); + for (unsigned i = count; i < PKT_MAX_REGS; i++) + _dma_buffer.regs[i] = 0x55aa; + + /* XXX implement check byte */ + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + result = -EINVAL; + perf_count(_pc_protoerrs); + } + + break; + } + perf_count(_pc_retries); + } + + sem_post(&_bus_semaphore); + + if (result == OK) + result = count; + return result; +} + +int +PX4IO_serial::read(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + uint16_t *values = reinterpret_cast<uint16_t *>(data); + + if (count > PKT_MAX_REGS) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = count | PKT_CODE_READ; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + result = -EINVAL; + perf_count(_pc_protoerrs); + + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != count) { + + /* IO returned the wrong number of registers - no point retrying */ + result = -EIO; + perf_count(_pc_protoerrs); + + /* successful read */ + } else { + + /* copy back the result */ + memcpy(values, &_dma_buffer.regs[0], (2 * count)); + } + + break; + } + perf_count(_pc_retries); + } + + sem_post(&_bus_semaphore); + + if (result == OK) + result = count; + return result; +} + +int +PX4IO_serial::_wait_complete() +{ + /* clear any lingering error status */ + (void)rSR; + (void)rDR; + + /* start RX DMA */ + perf_begin(_pc_txns); + perf_begin(_pc_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + + /* + * Note that we enable circular buffer mode as a workaround for + * there being no API to disable the DMA FIFO. We need direct mode + * because otherwise when the line idle interrupt fires there + * will be packet bytes still in the DMA FIFO, and we will assume + * that the idle was spurious. + * + * XXX this should be fixed with a NuttX change. + */ + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast<uint32_t>(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_CIRC | /* XXX see note above */ + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; + + /* start TX DMA - no callback if we also expect a reply */ + /* DMA setup time ~3µs */ + _dma_buffer.crc = 0; + _dma_buffer.crc = crc_packet(&_dma_buffer); + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast<uint32_t>(&_dma_buffer), + PKT_SIZE(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_tx_dma, nullptr, nullptr, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(_pc_dmasetup); + + /* compute the deadline for a 5ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); +#if 0 + abstime.tv_sec++; /* long timeout for testing */ +#else + abstime.tv_nsec += 10000000; /* 0ms timeout */ + if (abstime.tv_nsec > 1000000000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000000000; + } +#endif + + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); + + if (ret == OK) { + /* check for DMA errors */ + if (_rx_dma_status & DMA_STATUS_TEIF) { + perf_count(_pc_dmaerrs); + ret = -EIO; + break; + } + + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ + uint8_t crc = _dma_buffer.crc; + _dma_buffer.crc = 0; + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { + perf_count(_pc_crcerrs); + ret = -EIO; + break; + } + + /* successful txn (may still be reporting an error) */ + break; + } + + if (errno == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _abort_dma(); + perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ + break; + } + + /* we might? see this for EINTR */ + lowsyslog("unexpected ret %d/%d\n", ret, errno); + } + + /* reset DMA status */ + _rx_dma_status = _dma_status_inactive; + + /* update counters */ + perf_end(_pc_txns); + + return ret; +} + +void +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg); + + ps->_do_rx_dma_callback(status); + } +} + +void +PX4IO_serial::_do_rx_dma_callback(unsigned status) +{ + /* on completion of a reply, wake the waiter */ + if (_rx_dma_status == _dma_status_waiting) { + + /* check for packet overrun - this will occur after DMA completes */ + uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { + (void)rDR; + status = DMA_STATUS_TEIF; + } + + /* save RX status */ + _rx_dma_status = status; + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* complete now */ + sem_post(&_completion_semaphore); + } +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + if (g_interface != nullptr) + g_interface->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* read DR to clear status */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_status == _dma_status_waiting) { + _abort_dma(); + perf_count(_pc_uerrs); + + /* complete DMA as though in error */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & USART_SR_IDLE) { + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_status == _dma_status_waiting) { + + /* verify that the received packet is complete */ + unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { + perf_count(_pc_badidle); + return; + } + + perf_count(_pc_idle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TCIF); + } + } +} + +void +PX4IO_serial::_abort_dma() +{ + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* stop DMA */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); +} + +#endif /* PX4IO_SERIAL_BASE */
\ No newline at end of file diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 9e3f041e3..c7ce60b5e 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -52,51 +52,14 @@ #include <termios.h> #include <sys/stat.h> +#include <crc32.h> + #include "uploader.h" -static const uint32_t crctab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -static uint32_t -crc32(const uint8_t *src, unsigned len, unsigned state) -{ - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - return state; -} +#include <board_config.h> + +// define for comms logging +//#define UDEBUG PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), @@ -115,7 +78,11 @@ PX4IO_Uploader::upload(const char *filenames[]) const char *filename = NULL; size_t fw_size; - _io_fd = open("/dev/ttyS2", O_RDWR); +#ifndef PX4IO_SERIAL_DEVICE +#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload +#endif + + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { log("could not open interface"); @@ -258,12 +225,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { - //log("poll timeout %d", ret); +#ifdef UDEBUG + log("poll timeout %d", ret); +#endif return -ETIMEDOUT; } read(_io_fd, &c, 1); - //log("recv 0x%02x", c); +#ifdef UDEBUG + log("recv 0x%02x", c); +#endif return OK; } @@ -289,16 +260,20 @@ PX4IO_Uploader::drain() do { ret = recv(c, 1000); +#ifdef UDEBUG if (ret == OK) { - //log("discard 0x%02x", c); + log("discard 0x%02x", c); } +#endif } while (ret == OK); } int PX4IO_Uploader::send(uint8_t c) { - //log("send 0x%02x", c); +#ifdef UDEBUG + log("send 0x%02x", c); +#endif if (write(_io_fd, &c, 1) != 1) return -errno; @@ -572,14 +547,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) return -errno; /* calculate crc32 sum */ - sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum); + sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); bytes_read += count; } /* fill the rest with 0xff */ while (bytes_read < fw_size_remote) { - sum = crc32(&fill_blank, sizeof(fill_blank), sum); + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); bytes_read += sizeof(fill_blank); } diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk new file mode 100644 index 000000000..39b53ec9e --- /dev/null +++ b/src/drivers/rgbled/module.mk @@ -0,0 +1,6 @@ +# +# TCA62724FMG driver for RGB LED +# + +MODULE_COMMAND = rgbled +SRCS = rgbled.cpp diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp new file mode 100644 index 000000000..236f138a7 --- /dev/null +++ b/src/drivers/rgbled/rgbled.cpp @@ -0,0 +1,539 @@ +/**************************************************************************** + * + * 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 <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 <board_config.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, + LED_MODE_RGB +}; + +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; + + // RGB values for MODE_RGB + struct RGBLEDSet rgb; + + 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) { + 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: + case LED_MODE_RGB: + 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) { + case RGBLED_SET: { + /* set the specified RGB values */ + memcpy(&rgb, (struct RGBLEDSet *)arg, sizeof(rgb)); + setMode(LED_MODE_RGB); + return OK; + } + + default: + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = reinterpret_cast<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_RGB: + set_rgb(rgb.red, rgb.green, rgb.blue); + running = false; + return; + + 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 if (mode == LED_MODE_RGB) { + // no need to run again until the colour changes + set_on(true); + } 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', 'rgb'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + errx(0, " -a addr (0x%x)", ADDR); +} + +int +rgbled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(optarg, NULL, 0); + break; + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + default: + rgbled_usage(); + } + } + + const char *verb = argv[1]; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) + errx(1, "already started"); + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + if (g_rgbled == nullptr) { + // fall back to default bus + i2cdevice = PX4_I2C_BUS_LED; + } + } + if (g_rgbled == nullptr) { + 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); + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); + } + + if (!strcmp(verb, "test")) { + g_rgbled->setMode(LED_MODE_TEST); + exit(0); + } + + if (!strcmp(verb, "systemstate")) { + g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + exit(0); + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(verb, "off")) { + g_rgbled->setMode(LED_MODE_OFF); + exit(0); + } + + if (!strcmp(verb, "rgb")) { + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + if (argc < 5) { + errx(1, "Usage: rgbled rgb <red> <green> <blue>"); + } + struct RGBLEDSet v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); + close(fd); + exit(ret); + } + + rgbled_usage(); +} diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 1020eb946..00e46d6b8 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -227,7 +227,6 @@ ADC::init() if ((hrt_absolute_time() - now) > 500) { log("sample timeout"); return -1; - return 0xffff; } } @@ -282,7 +281,7 @@ ADC::close_last(struct file *filp) void ADC::_tick_trampoline(void *arg) { - ((ADC *)arg)->_tick(); + (reinterpret_cast<ADC *>(arg))->_tick(); } void @@ -342,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[8]; + adc_msg_s data[10]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) @@ -366,8 +365,15 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */ + g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | + (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 83a1a1abb..e79d7e10a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include <errno.h> #include <string.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include "chip.h" diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 2284be84d..24eec52af 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -104,7 +104,7 @@ #include <math.h> #include <ctype.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include <arch/stm32/chip.h> diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h new file mode 100644 index 000000000..600a65d28 --- /dev/null +++ b/src/include/device/rgbled.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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.h + * + * RGB led device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +/* more devices will be 1, 2, etc */ +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +/* + * ioctl() definitions + */ + +#define _RGBLEDIOCBASE (0x2900) +#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving <duration>, <command>, arg[0-2] + * + * The script is terminated by a zero command. + */ +#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) + +/** set constant RGB values */ +#define RGBLED_SET _RGBLEDIOC(4) + +/* + structure passed to RGBLED_SET ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +struct RGBLEDSet { + uint8_t red; + uint8_t green; + uint8_t blue; +}; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..9b6527c33 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); - up_systemreset(); + systemreset(false); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1aef739c7..7466dfdb9 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -109,19 +109,19 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[3], "a1")) { use_io = true; - pin = PX4IO_ACC1; + pin = PX4IO_P_SETUP_RELAYS_ACC1; } else if (!strcmp(argv[3], "a2")) { use_io = true; - pin = PX4IO_ACC2; + pin = PX4IO_P_SETUP_RELAYS_ACC2; } else if (!strcmp(argv[3], "r1")) { use_io = true; - pin = PX4IO_RELAY1; + pin = PX4IO_P_SETUP_RELAYS_POWER1; } else if (!strcmp(argv[3], "r2")) { use_io = true; - pin = PX4IO_RELAY2; + pin = PX4IO_P_SETUP_RELAYS_POWER2; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -142,7 +142,7 @@ int gpio_led_main(int argc, char *argv[]) char pin_name[24]; if (use_io) { - if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); } else { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index fbd82a4c6..796c6cd9f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -124,7 +124,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still @@ -197,7 +197,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + ASSERT(mapped < PX4IO_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ @@ -209,7 +209,7 @@ controls_tick() { } /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } @@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_CONTROL_CHANNELS) + *num_values = PX4IO_CONTROL_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 745cdfa40..206e27db5 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -53,13 +53,13 @@ #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ -static int dsm_fd = -1; /**<File handle to the DSM UART*/ -static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/ -static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/ -static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/ -static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/ -static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/ -static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/ +static int dsm_fd = -1; /**< File handle to the DSM UART */ +static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */ +static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ +static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */ +static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */ +static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ +static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ /** * Attempt to decode a single channel raw channel datum @@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses) if (dsm_fd < 0) return; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // XXX implement + #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 +#else switch (cmd) { case dsm_bind_power_down: @@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } +#endif } /** diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..10aeb5c9f 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 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 @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,18 @@ i2c_init(void) #endif } +void +interface_tick() +{ +} + /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +337,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..d8c0e58ba 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,7 +154,7 @@ mixer_tick(void) if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ @@ -164,11 +164,11 @@ mixer_tick(void) } else if (source != MIX_NONE) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -180,7 +180,7 @@ mixer_tick(void) r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } @@ -215,7 +215,7 @@ mixer_tick(void) if (mixer_servos_armed) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } } @@ -349,11 +349,11 @@ mixer_set_failsafe() return; /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -364,7 +364,7 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servo_failsafe[i] = 0; } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f4..59f470a94 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,17 +3,22 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -
\ No newline at end of file + +ifeq ($(BOARD),px4io-v1) +SRCS += i2c.c +endif +ifeq ($(BOARD),px4io-v2) +SRCS += serial.c \ + ../systemlib/hx_stream.c +endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 88d8cc87c..3c59a75a7 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -45,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - /* Per C, this is safe for all 2's complement systems */ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) @@ -75,10 +74,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PROTOCOL_VERSION 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ @@ -108,16 +109,20 @@ #define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -143,7 +148,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -156,33 +161,34 @@ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -/* px4io relay bit definitions */ -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ -enum { /* DSM bind states */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ dsm_bind_power_down = 0, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart }; + /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -194,10 +200,14 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ /** * As-needed mixer data upload. @@ -218,3 +228,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..e70b3fe88 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -64,11 +64,6 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif - /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -147,8 +142,10 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif /* start the safety switch handler */ safety_init(); @@ -159,10 +156,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 - /* start the i2c handler */ - i2c_init(); -#endif + /* start the FMU interface */ + interface_init(); + + /* add a performance counter for the interface */ + perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -205,6 +203,11 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); + /* kick the interface */ + perf_begin(interface_perf); + interface_tick(); + perf_end(interface_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -223,12 +226,11 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 57cffcc23..b30d155bc 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,15 +42,16 @@ #include <stdbool.h> #include <stdint.h> -#include <drivers/boards/px4io/px4io_internal.h> +#include <board_config.h> #include "protocol.h" /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 /* * Debug logging @@ -119,32 +120,43 @@ extern struct sys_state_s system_state; /* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif -#ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - /* * Mixer */ @@ -156,17 +168,16 @@ extern void mixer_handle_text(const void *buffer, size_t length); */ extern void safety_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** @@ -191,10 +202,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ +/** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); - -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a922362b6..3f241d29c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -44,6 +44,7 @@ #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include "px4io.h" #include "protocol.h" @@ -57,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -79,7 +84,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0 }; /** @@ -87,14 +95,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -104,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -114,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -138,7 +146,11 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, }; @@ -147,7 +159,7 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -166,7 +178,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -182,9 +194,9 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; -void +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -233,7 +245,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; @@ -260,11 +272,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -349,10 +363,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); - POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); - POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); - POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); +#ifdef POWER_RELAY1 + POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); +#endif +#ifdef POWER_RELAY2 + POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); +#endif +#ifdef POWER_ACC1 + POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); +#endif +#ifdef POWER_ACC2 + POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); +#endif + break; + + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: @@ -381,7 +407,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -426,7 +452,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } @@ -446,6 +472,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } @@ -482,6 +516,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -515,7 +550,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -525,26 +561,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * XXX pending measurements + * + * slope = xxx + * intercept = xxx + * + * Intercept corrected for best results @ 5.0V. + */ + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + } + } +#endif + /* XXX PX4IO_P_STATUS_VRSSI */ + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); - +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif + +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -616,7 +688,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..94d7407df --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,352 @@ +/**************************************************************************** + * + * Copyright (C) 2012,2013 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 serial.c + * + * Serial communication for the PX4IO module. + */ + +#include <stdint.h> +#include <unistd.h> +#include <termios.h> +#include <fcntl.h> +#include <string.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> +#include <stm32.h> +#include <systemlib/perf_counter.h> + +//#define DEBUG +#include "px4io.h" + +static perf_counter_t pc_txns; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; +static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; + +static void rx_handle_packet(void); +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + +/* if we spend this many ticks idle, reset the DMA */ +static unsigned idle_ticks; + +static struct IOPacket dma_packet; + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +void +interface_init(void) +{ + pc_txns = perf_alloc(PC_ELAPSED, "txns"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); + + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* clear status/errors */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif + + /* configure RX DMA and return to listening state */ + dma_reset(); + + debug("serial init"); +} + +void +interface_tick() +{ + /* XXX look for stuck/damaged DMA and reset? */ + if (idle_ticks++ > 100) { + dma_reset(); + idle_ticks = 0; + } +} + +static void +rx_handle_packet(void) +{ + /* check packet CRC */ + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { + perf_count(pc_crcerr); + + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { + + /* it's a blind write - pass it on */ + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { + + /* it's a read - get register pointer for reply */ + unsigned count; + uint16_t *registers; + + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count * 2); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; + } + + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ + perf_begin(pc_txns); + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* reset the idle counter */ + idle_ticks = 0; + + /* handle the received packet */ + rx_handle_packet(); + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + dma_reset(); + + /* send the reply to the just-processed request */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(&dma_packet); + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + PKT_SIZE(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + stm32_dmastart(tx_dma, NULL, NULL, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); +} + +static int +serial_interrupt(int irq, void *context) +{ + static bool abort_on_idle = false; + + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; + + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; + } + + if (sr & USART_SR_IDLE) { + + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ + perf_count(pc_badidle); + return 0; + } + + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ + perf_count(pc_idle); + stm32_dmastop(rx_dma); + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} + diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bd431c9eb..8d3992963 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,10 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); + +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -158,8 +161,12 @@ PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b971..c47f6bb7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include <stdio.h> #include <errno.h> #include <math.h> +#include <mathlib/mathlib.h> #include <nuttx/analog/adc.h> @@ -138,6 +139,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** * Sensor app start / stop handling function * * @ingroup apps @@ -189,8 +261,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -210,13 +282,15 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -227,6 +301,9 @@ private: float accel_scale[3]; float diff_pres_offset_pa; + int board_rotation; + int external_mag_rotation; + int rc_type; int rc_map_roll; @@ -265,7 +342,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -306,6 +382,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -315,6 +394,11 @@ private: int parameters_update(); /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + + /** * Do accel-related initialisation. */ void accel_init(); @@ -450,7 +534,10 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3) { /* basic r/c parameters */ @@ -540,6 +627,10 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -731,10 +822,34 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + +void Sensors::accel_init() { int fd; @@ -757,7 +872,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -765,6 +880,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); @@ -874,9 +992,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +1018,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +1044,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + // XXX TODO add support for external mag orientation + + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 3283aad8a..57a751e1c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -43,13 +43,29 @@ #include <fcntl.h> #include <sched.h> #include <signal.h> -#include <sys/stat.h> #include <unistd.h> #include <float.h> #include <string.h> +#include <sys/stat.h> +#include <sys/types.h> + +#include <stm32_pwr.h> + #include "systemlib.h" +void +systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } + up_systemreset(); +} + static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..3728f2067 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include <float.h> #include <stdint.h> -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +__EXPORT void systemreset(bool to_bootloader) noreturn_function; + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c index 49da51358..2aed80e01 100644 --- a/src/systemcmds/eeprom/eeprom.c +++ b/src/systemcmds/eeprom/eeprom.c @@ -55,7 +55,7 @@ #include <nuttx/fs/nxffs.h> #include <nuttx/fs/ioctl.h> -#include <arch/board/board.h> +#include <board_config.h> #include "systemlib/systemlib.h" #include "systemlib/param/param.h" diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 4da238039..34405c496 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -52,7 +52,7 @@ #include <nuttx/i2c.h> -#include <arch/board/board.h> +#include <board_config.h> #include "systemlib/systemlib.h" #include "systemlib/err.h" diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk new file mode 100644 index 000000000..a48588535 --- /dev/null +++ b/src/systemcmds/nshterm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Build nshterm utility +# + +MODULE_COMMAND = nshterm +SRCS = nshterm.c + +MODULE_STACKSIZE = 3000 diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c new file mode 100644 index 000000000..bde0e7841 --- /dev/null +++ b/src/systemcmds/nshterm/nshterm.c @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Andrew Tridgell + * + * 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 nshterm.c + * start a nsh terminal on a given port. This can be useful for error + * handling in startup scripts to start a nsh shell on /dev/ttyACM0 + * for diagnostics + */ + +#include <nuttx/config.h> +#include <stdbool.h> +#include <stdio.h> +#include <stdarg.h> +#include <unistd.h> +#include <stdlib.h> +#include <errno.h> +#include <apps/nsh.h> +#include <fcntl.h> + +__EXPORT int nshterm_main(int argc, char *argv[]); + +int +nshterm_main(int argc, char *argv[]) +{ + if (argc < 2) { + printf("Usage: nshterm <device>\n"); + exit(1); + } + uint8_t retries = 0; + int fd = -1; + while (retries < 5) { + // the retries are to cope with the behaviour of /dev/ttyACM0 + // which may not be ready immediately. + fd = open(argv[1], O_RDWR); + if (fd != -1) { + break; + } + usleep(100000); + retries++; + } + if (fd == -1) { + perror(argv[1]); + exit(1); + } + // setup standard file descriptors + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; +} diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 47d3cd948..91a2c2eb8 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -40,14 +40,31 @@ #include <nuttx/config.h> #include <unistd.h> #include <stdio.h> +#include <getopt.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - up_systemreset(); + int ch; + bool to_bootloader = false; + + while ((ch = getopt(argc, argv, "b")) != -1) { + switch (ch) { + case 'b': + to_bootloader = true; + break; + default: + errx(1, "usage: reboot [-b]\n" + " -b reboot into the bootloader"); + + } + } + + systemreset(to_bootloader); } |