From bd580e09bfbd3200b04ee41bacbbae1c69c6fde6 Mon Sep 17 00:00:00 2001 From: Felix Hu Date: Sat, 12 Mar 2016 12:50:14 +0800 Subject: [PATCH] supports MindPXv2 borad which is a product from AirMind. --- Firmware.sublime-project | 7 + Images/mindpx-v2.prototype | 12 + Makefile | 3 + ROMFS/px4fmu_common/init.d/rc.interface | 6 + ROMFS/px4fmu_common/init.d/rc.sensors | 50 +- ROMFS/px4fmu_common/init.d/rcS | 99 +- cmake/configs/nuttx_mindpx-v2_default.cmake | 198 ++ cmake/nuttx/px4_impl_nuttx.cmake | 8 + nuttx-configs/mindpx-v2/include/board.h | 310 +++ .../mindpx-v2/include/nsh_romfsimg.h | 42 + nuttx-configs/mindpx-v2/nsh/Make.defs | 177 ++ nuttx-configs/mindpx-v2/nsh/appconfig | 52 + nuttx-configs/mindpx-v2/nsh/defconfig | 1066 ++++++++ nuttx-configs/mindpx-v2/nsh/setenv.sh | 67 + nuttx-configs/mindpx-v2/scripts/ld.script | 159 ++ nuttx-configs/mindpx-v2/src/Makefile | 84 + nuttx-configs/mindpx-v2/src/empty.c | 4 + src/drivers/boards/mindpx-v2/CMakeLists.txt | 47 + src/drivers/boards/mindpx-v2/board_config.h | 340 +++ src/drivers/boards/mindpx-v2/mindpx2_init.c | 374 +++ src/drivers/boards/mindpx-v2/mindpx2_led.c | 96 + src/drivers/boards/mindpx-v2/mindpx_can.c | 144 + src/drivers/boards/mindpx-v2/mindpx_spi.c | 226 ++ .../boards/mindpx-v2/mindpx_timer_config.c | 131 + src/drivers/boards/mindpx-v2/mindpx_usb.c | 108 + src/drivers/drv_gpio.h | 3 +- src/drivers/drv_sensor.h | 8 + src/drivers/hc_sr04/CMakeLists.txt | 44 + src/drivers/hc_sr04/hc_sr04.cpp | 987 +++++++ src/drivers/l3gd20/l3gd20.cpp | 14 +- src/drivers/lsm303d/lsm303d.cpp | 22 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 3 +- src/drivers/mpu6500/CMakeLists.txt | 45 + src/drivers/mpu6500/mpu6500.cpp | 2412 +++++++++++++++++ src/drivers/px4fmu/fmu.cpp | 121 +- src/drivers/rgbled_pwm/CMakeLists.txt | 44 + src/drivers/rgbled_pwm/drv_led_pwm.cpp | 382 +++ src/drivers/rgbled_pwm/rgbled_pwm.cpp | 697 +++++ src/drivers/srf02_i2c/CMakeLists.txt | 43 + src/drivers/srf02_i2c/srf02_i2c.cpp | 961 +++++++ src/drivers/stm32/adc/adc.cpp | 12 +- src/drivers/test_ppm/CMakeLists.txt | 45 + src/drivers/test_ppm/test_ppm.cpp | 290 ++ src/lib/version/version.h | 4 + src/modules/commander/esc_calibration.cpp | 15 +- src/modules/commander/esc_calibration.h | 3 +- .../commander/state_machine_helper.cpp | 12 +- src/modules/gpio_led/gpio_led.c | 9 +- src/modules/sensors/sensors.cpp | 4 +- 49 files changed, 9941 insertions(+), 49 deletions(-) create mode 100644 Images/mindpx-v2.prototype create mode 100644 cmake/configs/nuttx_mindpx-v2_default.cmake create mode 100644 nuttx-configs/mindpx-v2/include/board.h create mode 100644 nuttx-configs/mindpx-v2/include/nsh_romfsimg.h create mode 100644 nuttx-configs/mindpx-v2/nsh/Make.defs create mode 100644 nuttx-configs/mindpx-v2/nsh/appconfig create mode 100644 nuttx-configs/mindpx-v2/nsh/defconfig create mode 100644 nuttx-configs/mindpx-v2/nsh/setenv.sh create mode 100644 nuttx-configs/mindpx-v2/scripts/ld.script create mode 100644 nuttx-configs/mindpx-v2/src/Makefile create mode 100644 nuttx-configs/mindpx-v2/src/empty.c create mode 100644 src/drivers/boards/mindpx-v2/CMakeLists.txt create mode 100644 src/drivers/boards/mindpx-v2/board_config.h create mode 100644 src/drivers/boards/mindpx-v2/mindpx2_init.c create mode 100644 src/drivers/boards/mindpx-v2/mindpx2_led.c create mode 100644 src/drivers/boards/mindpx-v2/mindpx_can.c create mode 100644 src/drivers/boards/mindpx-v2/mindpx_spi.c create mode 100644 src/drivers/boards/mindpx-v2/mindpx_timer_config.c create mode 100644 src/drivers/boards/mindpx-v2/mindpx_usb.c create mode 100644 src/drivers/hc_sr04/CMakeLists.txt create mode 100644 src/drivers/hc_sr04/hc_sr04.cpp create mode 100644 src/drivers/mpu6500/CMakeLists.txt create mode 100644 src/drivers/mpu6500/mpu6500.cpp create mode 100644 src/drivers/rgbled_pwm/CMakeLists.txt create mode 100644 src/drivers/rgbled_pwm/drv_led_pwm.cpp create mode 100644 src/drivers/rgbled_pwm/rgbled_pwm.cpp create mode 100644 src/drivers/srf02_i2c/CMakeLists.txt create mode 100644 src/drivers/srf02_i2c/srf02_i2c.cpp create mode 100644 src/drivers/test_ppm/CMakeLists.txt create mode 100644 src/drivers/test_ppm/test_ppm.cpp diff --git a/Firmware.sublime-project b/Firmware.sublime-project index c9cb9cfc02..bfdddda2b9 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -70,6 +70,13 @@ "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", "cmd": ["make posix"], "shell": true + }, + { + "name": "MindPX_V2: make and upload", + "working_dir": "${project_path}", + "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "cmd": ["make upload mindpx-v2_default -j8"], + "shell": true } ] } diff --git a/Images/mindpx-v2.prototype b/Images/mindpx-v2.prototype new file mode 100644 index 0000000000..aa7182d623 --- /dev/null +++ b/Images/mindpx-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the MindPx-V2 board", + "image": "", + "build_time": 0, + "summary": "MindPx-v2", + "version": "2.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index 188486b5c1..c88f3160b6 100644 --- a/Makefile +++ b/Makefile @@ -143,6 +143,9 @@ px4fmu-v2_ekf2: px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) +mindpx-v2_default: + $(call cmake-build,nuttx_mindpx-v2_default) + posix_sitl_default: $(call cmake-build,$@) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 2e58871622..a8c17ad2c0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -103,6 +103,12 @@ then set MIXER_AUX none fi +#MindPX has not aux mixer +if ver hwcmp MINDPX_V2 +then + set MIXER_AUX none +fi + if [ $MIXER_AUX != none -a $AUX_MODE != none ] then # diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 4082f8eab3..d0ab828bb9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -106,32 +106,52 @@ else then fi else - # FMUv1 - if mpu6000 start + if ver hwcmp MINDPX_V2 then - fi + if mpu6500 start + then + fi - if l3gd20 start - then - fi + if lsm303d start + then + fi - # MAG selection - if param compare SENS_EXT_MAG 2 - then - if hmc5883 -C -I start + if l3gd20 start + then + fi + + # External I2C bus + if hmc5883 -C -T -X start then fi else - # Use only external as primary - if param compare SENS_EXT_MAG 1 + # FMUv1 + if mpu6000 start then - if hmc5883 -C -X start + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 + then + if hmc5883 -C -I start then fi else - # auto-detect the primary, prefer external - if hmc5883 start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 48ca242956..09da09fc57 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -114,21 +114,6 @@ then set AUTOCNF no fi - # - # Set USE_IO flag - # - if param compare SYS_USE_IO 1 - then - if ver hwcmp PX4FMU_V4 - then - set USE_IO no - else - set USE_IO yes - fi - else - set USE_IO no - fi - # # Set default values # @@ -156,6 +141,35 @@ then set EXIT_ON_END no set MAV_TYPE none set FAILSAFE none + set USE_IO yes + + # + # Set USE_IO flag + # + if param compare SYS_USE_IO 1 + then + if ver hwcmp PX4FMU_V4 + then + set USE_IO no + fi + + if ver hwcmp MINDPX_V2 + then + set USE_IO no + fi + else + set USE_IO no + fi + + # should set to 0.8 for mindpx-v2 borad. + if param compare INAV_LIDAR_ERR 0.5 + then + if ver hwcmp MINDPX_V2 + then + param set INAV_LIDAR_ERR 0.8 + param save + fi + fi # # Set parameters and env variables for selected AUTOSTART @@ -526,6 +540,49 @@ then fi fi + # + # MAVLink onboard / TELEM2 + # + if ver hwcmp MINDPX_V2 + then + else + # XXX We need a better way for runtime eval of shell variables, + # but this works for now + if param compare SYS_COMPANION 921600 + then + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x + fi + if param compare SYS_COMPANION 57600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x + fi + if param compare SYS_COMPANION 157600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 + fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi + if param compare SYS_COMPANION 357600 + then + mavlink start -d /dev/ttyS2 -b 57600 -r 1000 + fi + # Sensors on the PWM interface bank + # clear pins 5 and 6 + if param compare SENS_EN_LL40LS 1 + then + set AUX_MODE pwm4 + fi + if param greater TRIG_MODE 0 + then + # Get FMU driver out of the way + set MIXER_AUX none + set AUX_MODE none + camera_trigger start + fi + fi + # Transitional support: Disable safety on all Pixracer boards if ver hwcmp PX4FMU_V4 then @@ -801,6 +858,12 @@ fi # XXX potentially unset all script variables. unset TUNE_ERR +#if ver hwcmp MINDPX_V2 +#then +# start sonnar +# hc_sr04 start +#fi + # Boot is complete, inform MAVLink app(s) that the system is now fully up and running mavlink boot_complete @@ -826,6 +889,12 @@ then px4flow start & fi +if ver hwcmp MINDPX_V2 +then + #mindxp also need flow + px4flow start & +fi + # Start USB shell if no microSD present, MAVLink else if [ $LOG_FILE == /dev/null ] then diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake new file mode 100644 index 0000000000..0120f416fb --- /dev/null +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -0,0 +1,198 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_uavcan_num_ifaces 2) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + #drivers/px4io + drivers/test_ppm + drivers/boards/mindpx-v2 + #drivers/rgbled + drivers/rgbled_pwm + #drivers/mpu6000 + #drivers/mpu6050 + drivers/mpu6500 + #drivers/mpu9250 + drivers/lsm303d + drivers/l3gd20 + drivers/hmc5883 + drivers/ms5611 + #drivers/mb12xx + #drivers/srf02 + drivers/srf02_i2c + drivers/hc_sr04 + #drivers/sf0x + #drivers/ll40ls + #drivers/trone + drivers/gps + drivers/pwm_out_sim + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + #drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + drivers/bst + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib + systemcmds/reboot + #systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + modules/dataman + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + #modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_io_board + px4io-v2 + ) + +set(config_extra_libs + ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 783886e373..0af6458e48 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -483,6 +483,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "mindpx-v2") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "px4io-v1") set(cpu_flags -mcpu=cortex-m3 diff --git a/nuttx-configs/mindpx-v2/include/board.h b/nuttx-configs/mindpx-v2/include/board.h new file mode 100644 index 0000000000..8449510e33 --- /dev/null +++ b/nuttx-configs/mindpx-v2/include/board.h @@ -0,0 +1,310 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MINDPX uses a 8MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + * CAN2 is routed to the expansion connector. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) + +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/mindpx-v2/include/nsh_romfsimg.h b/nuttx-configs/mindpx-v2/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/mindpx-v2/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/mindpx-v2/nsh/Make.defs b/nuttx-configs/mindpx-v2/nsh/Make.defs new file mode 100644 index 0000000000..a09e6794e3 --- /dev/null +++ b/nuttx-configs/mindpx-v2/nsh/Make.defs @@ -0,0 +1,177 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -Os +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/mindpx-v2/nsh/appconfig b/nuttx-configs/mindpx-v2/nsh/appconfig new file mode 100644 index 0000000000..0e18aa8ef1 --- /dev/null +++ b/nuttx-configs/mindpx-v2/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/mindpx-v2/nsh/defconfig b/nuttx-configs/mindpx-v2/nsh/defconfig new file mode 100644 index 0000000000..6fccd51c4c --- /dev/null +++ b/nuttx-configs/mindpx-v2/nsh/defconfig @@ -0,0 +1,1066 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=n +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +CONFIG_STM32_SPI4=y +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +CONFIG_STM32_USART6=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +# CONFIG_ARCH_IRQPRIO is not set +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=1500 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +#CONFIG_ARCH_BOARD_PX4FMU_V2=y +CONFIG_ARCH_BOARD_MINDPX_V2=y +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HSECLOCK=y +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +#CONFIG_USART3_IFLOWCONTROL=y +#CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=1000 +CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_USBDEV_TRACE is not set +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/mindpx-v2/nsh/setenv.sh b/nuttx-configs/mindpx-v2/nsh/setenv.sh new file mode 100644 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/mindpx-v2/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/mindpx-v2/scripts/ld.script b/nuttx-configs/mindpx-v2/scripts/ld.script new file mode 100644 index 0000000000..b04ad89a6c --- /dev/null +++ b/nuttx-configs/mindpx-v2/scripts/ld.script @@ -0,0 +1,159 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/mindpx-v2/src/Makefile b/nuttx-configs/mindpx-v2/src/Makefile new file mode 100644 index 0000000000..d4276f7fc5 --- /dev/null +++ b/nuttx-configs/mindpx-v2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/mindpx-v2/src/empty.c b/nuttx-configs/mindpx-v2/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/mindpx-v2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/mindpx-v2/CMakeLists.txt b/src/drivers/boards/mindpx-v2/CMakeLists.txt new file mode 100644 index 0000000000..306c3ab0b0 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__mindpx-v2 + COMPILE_FLAGS + -Os + SRCS + mindpx_can.c + mindpx2_init.c + mindpx_timer_config.c + mindpx_spi.c + mindpx_usb.c + mindpx2_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h new file mode 100644 index 0000000000..b07949e1a6 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/board_config.h @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * MINDPXv2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * 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_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) + +/* External interrupts */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN4) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN13) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN0) + +/* Data ready pins off */ +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTE|GPIO_PIN14) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTC|GPIO_PIN14) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTC|GPIO_PIN13) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTC|GPIO_PIN0) + + + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) + +/*SPI1 chip selects off */ +#define GPIO_SPI_CS_FRAM_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTC|GPIO_PIN2) + +/* SPI1 chip selects */ +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + + + +/* SPI4 off */ +#define GPIO_SPI4_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI4_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN5) +#define GPIO_SPI4_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN6) + +/* SPI4 chip selects off */ +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTD|GPIO_PIN12) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTD|GPIO_PIN11) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTE|GPIO_PIN3) + +/* SPI4 chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN12) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + + + + + +/* SPI2 off */ +#define GPIO_SPI2_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN13) +#define GPIO_SPI2_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN14) +#define GPIO_SPI2_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) + +/* SPI2 chip selects off */ +#define GPIO_SPI_CS_EXT0_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTB|GPIO_PIN7) +#define GPIO_SPI_CS_EXT1_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTB|GPIO_PIN5) +#define GPIO_SPI_CS_EXT2_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTB|GPIO_PIN3) +#define GPIO_SPI_CS_EXT3_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_25MHz|GPIO_PORTD|GPIO_PIN7) +/* SPI2 chip selects */ +#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN7) +#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) +#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) +#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_25MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) + +#define PX4_SPI_BUS_SENSORS 4 +#define PX4_SPI_BUS_RAMTRON 1 +#define PX4_SPI_BUS_EXT 2 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS + +/* 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 +#define PX4_SPIDEV_MPU 4 + + +#define PX4_SPIDEV_FLASH 5 + + +/* External bus */ +#define PX4_SPIDEV_EXT0 1 +#define PX4_SPIDEV_EXT1 2 +#define PX4_SPIDEV_EXT2 3 +#define PX4_SPIDEV_EXT3 4 + +/* FMUv3 SPI on external bus */ +//#define PX4_SPIDEV_EXT_MPU PX4_SPIDEV_EXT0 +//#define PX4_SPIDEV_EXT_BARO PX4_SPIDEV_EXT1 +//#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_SPIDEV_EXT2 +//#define PX4_SPIDEV_EXT_GYRO PX4_SPIDEV_EXT3 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 2 +#define PX4_I2C_BUS_ONBOARD 1 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +//#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e +//#define PX4_I2C_OBDEV_MPU6050 0x68 + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 3 +#define ADC_BATTERY_CURRENT_CHANNEL 2 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#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_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN10) +#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN12) +#define GPIO_GPIO12_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) +#define GPIO_GPIO13_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN6) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#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) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN10) +#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1) +#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN10) +#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN10) +#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_GPIO12_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN15) +#define GPIO_GPIO13_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) + + +/* 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 : PE9 : TIM1_CH1 + * CH2 : PE11 : TIM1_CH2 + * CH3 : PE13 : TIM1_CH3 + * CH4 : PE14 : TIM1_CH4 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN9) +#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN11) +#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN13) +#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN14) +#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN13) +#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN14) +#define DIRECT_PWM_OUTPUT_CHANNELS 6 + +#define GPIO_TIM1_CH1IN GPIO_TIM1_CH1IN_2 +#define GPIO_TIM1_CH2IN GPIO_TIM1_CH2IN_2 +#define GPIO_TIM1_CH3IN GPIO_TIM1_CH3IN_2 +#define GPIO_TIM1_CH4IN GPIO_TIM1_CH4IN_2 +#define GPIO_TIM4_CH2IN GPIO_TIM4_CH2IN_2 +#define GPIO_TIM4_CH3IN GPIO_TIM4_CH3IN_2 +#define DIRECT_INPUT_TIMER_CHANNELS 6 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +//#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + + + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel */ + +#define HRT_PPM_CHANNEL 1 +#define GPIO_PPM_IN GPIO_TIM8_CH1IN_1 +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 3 +#define PWMIN_TIMER_CHANNEL 1 +#define GPIO_PWM_IN GPIO_TIM3_CH1IN_3 + +/**************************************************************************************************** + * 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); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/mindpx-v2/mindpx2_init.c b/src/drivers/boards/mindpx-v2/mindpx2_init.c new file mode 100644 index 0000000000..1383485d7d --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx2_init.c @@ -0,0 +1,374 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mindpx2_init.c + * + * MINDPX-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * 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(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct spi_dev_s *spi4; +static struct sdio_dev_s *sdio; + +#include + +#if 0 +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + 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 the DMA allocator */ + dma_alloc_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 */ + message("[boot] Initialized SPI port 4 (SENSORS)\n"); + spi4 = up_spiinitialize(4); + + if (!spi4) { + message("[boot] FAILED to initialize SPI port 4\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI4 to 10MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi4, 10000000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi4, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi4, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi4, PX4_SPIDEV_MPU, false); + up_udelay(20); + + /* Get the SPI port for the FRAM */ + message("[boot] Initialized SPI port 1 (RAMTRON FRAM)\n"); + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi1, 24 * 1000 * 1000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, SPIDEV_FLASH, false); + + + + message("[boot] Initialized SPI port 2 (nRF24 and ext)\n"); + + spi2 = up_spiinitialize(2); + + /* Default SPI2 to 10MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi2, PX4_SPIDEV_EXT1, false); + SPI_SELECT(spi2, PX4_SPIDEV_EXT2, false); + SPI_SELECT(spi2, PX4_SPIDEV_EXT3, false); + +#ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + message("[boot] 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("[boot] 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); + +#endif + + + stm32_configgpio(GPIO_I2C2_SCL); + stm32_configgpio(GPIO_I2C2_SDA); + message("[boot] Initialized ext I2C Port\n"); + + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + message("[boot] Initialized onboard I2C Port\n"); + + + + return OK; +} diff --git a/src/drivers/boards/mindpx-v2/mindpx2_led.c b/src/drivers/boards/mindpx-v2/mindpx2_led.c new file mode 100644 index 0000000000..5c3985d7d3 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx2_led.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mindpx2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * 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(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(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); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { + stm32_gpiowrite(GPIO_LED1, false); + + } else { + stm32_gpiowrite(GPIO_LED1, true); + } + } +} diff --git a/src/drivers/boards/mindpx-v2/mindpx_can.c b/src/drivers/boards/mindpx-v2/mindpx_can.c new file mode 100644 index 0000000000..58f8023597 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mindpx_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "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/mindpx-v2/mindpx_spi.c b/src/drivers/boards/mindpx-v2/mindpx_spi.c new file mode 100644 index 0000000000..012455b604 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx_spi.c @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mindpx_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#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_SPI4 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); +// stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* 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); +// stm32_gpiowrite(GPIO_SPI_CS_FRAM,1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif + +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_EXT0); + stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_configgpio(GPIO_SPI_CS_EXT2); + stm32_configgpio(GPIO_SPI_CS_EXT3); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); +#endif +} + +__EXPORT void stm32_spi4select(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); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 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); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 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); +// stm32_gpiowrite(GPIO_SPI_CS_FRAM,1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + +// case PX4_SPIDEV_FLASH: +// stm32_gpiowrite(GPIO_SPI_CS_BARO,1); +// stm32_gpiowrite(GPIO_SPI_CS_FRAM,!selected); +// break; + + case PX4_SPIDEV_MPU: + /* 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, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI1 +__EXPORT void stm32_spi1select(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_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif + +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT2: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT3: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} diff --git a/src/drivers/boards/mindpx-v2/mindpx_timer_config.c b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c new file mode 100644 index 0000000000..5a6167691c --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file mindpx_timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM1CC, + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .first_channel_index = 4, + .last_channel_index = 5, + .handler = io_timer_handler1, + .vectorno = STM32_IRQ_TIM4 + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM1_CH1OUT, + .gpio_in = GPIO_TIM1_CH1IN, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM1_CH2OUT, + .gpio_in = GPIO_TIM1_CH2IN, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM1_CH3OUT, + .gpio_in = GPIO_TIM1_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM1_CH4OUT, + .gpio_in = GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + }, + { + .gpio_out = GPIO_TIM4_CH2OUT, + .gpio_in = GPIO_TIM4_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM4_CH3OUT, + .gpio_in = GPIO_TIM4_CH3IN, + .timer_index = 1, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + } +// { +// .gpio = GPIO_TIM4_CH4OUT, +// .timer_index = 1, +// .timer_channel = 4, +// .default_value = 1000, +// } +}; diff --git a/src/drivers/boards/mindpx-v2/mindpx_usb.c b/src/drivers/boards/mindpx-v2/mindpx_usb.c new file mode 100644 index 0000000000..1cbc5649c4 --- /dev/null +++ b/src/drivers/boards/mindpx-v2/mindpx_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mindpx_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#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/drv_gpio.h b/src/drivers/drv_gpio.h index 971c22b2d4..5d320fe7e6 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -66,7 +66,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_MINDPX_V2) /* * PX4FMUv2 GPIO numbers. * @@ -144,6 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ + !defined(CONFIG_ARCH_BOARD_MINDPX_V2) &&\ !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index abe876244d..95a9c974b5 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -69,6 +69,14 @@ #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 +#ifdef CONFIG_ARCH_BOARD_MINDPX_V2 +#define DRV_ACC_DEVTYPE_MPU6050 0x14 +#define DRV_ACC_DEVTYPE_MPU6500 0x13 + +#define DRV_GYR_DEVTYPE_MPU6050 0x20 +#define DRV_GYR_DEVTYPE_MPU6500 0x21 +#endif + /* * ioctl() definitions * diff --git a/src/drivers/hc_sr04/CMakeLists.txt b/src/drivers/hc_sr04/CMakeLists.txt new file mode 100644 index 0000000000..9a493c4db8 --- /dev/null +++ b/src/drivers/hc_sr04/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__hc_sr04 + MAIN hc_sr04 + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + hc_sr04.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/hc_sr04/hc_sr04.cpp b/src/drivers/hc_sr04/hc_sr04.cpp new file mode 100644 index 0000000000..e6869933d6 --- /dev/null +++ b/src/drivers/hc_sr04/hc_sr04.cpp @@ -0,0 +1,987 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hc_sr04.cpp + * + * Driver for the hc_sr04 sonar range finders . + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + + +#include + + +#define SR04_MAX_RANGEFINDERS 6 +#define SR04_ID_BASE 0x10 + +/* Configuration Constants */ +#define SR04_DEVICE_PATH "/dev/hc_sr04" + +#define SUBSYSTEM_TYPE_RANGEFINDER 131072 +/* Device limits */ +#define SR04_MIN_DISTANCE (0.10f) +#define SR04_MAX_DISTANCE (4.00f) + +#define SR04_CONVERSION_INTERVAL 100000 /* 100ms for one sonar */ + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class HC_SR04 : public device::CDev +{ +public: + HC_SR04(unsigned sonars = 6); + virtual ~HC_SR04(); + + 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(); + void interrupt(unsigned time); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + unsigned _sonars; + struct GPIOConfig { + uint32_t trig_port; + uint32_t echo_port; + uint32_t alt; + }; + static const GPIOConfig _gpio_tab[]; + unsigned _raising_time; + unsigned _falling_time; + unsigned _status; + /** + * 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. + * + * @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. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +const HC_SR04::GPIOConfig HC_SR04::_gpio_tab[] = { + {GPIO_GPIO6_OUTPUT, GPIO_GPIO7_INPUT, 0}, + {GPIO_GPIO6_OUTPUT, GPIO_GPIO8_INPUT, 0}, + {GPIO_GPIO6_OUTPUT, GPIO_GPIO9_INPUT, 0}, + {GPIO_GPIO6_OUTPUT, GPIO_GPIO10_INPUT, 0}, + {GPIO_GPIO6_OUTPUT, GPIO_GPIO11_INPUT, 0}, + {GPIO_GPIO6_OUTPUT, GPIO_GPIO12_INPUT, 0} +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int hc_sr04_main(int argc, char *argv[]); +static int sonar_isr(int irq, void *context); + +HC_SR04::HC_SR04(unsigned sonars) : + CDev("HC_SR04", SR04_DEVICE_PATH, 0), + _min_distance(SR04_MIN_DISTANCE), + _max_distance(SR04_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "hc_sr04_read")), + _comms_errors(perf_alloc(PC_COUNT, "hc_sr04_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "hc_sr04_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0), /* initialising temp sonar i2c address to zero */ + _sonars(sonars), + _raising_time(0), + _falling_time(0), + _status(0) + +{ + /* enable debug() calls */ + _debug_enabled = false; + + /* work_cancel in the dtor will explode if we don't do this... */ + memset(&_work, 0, sizeof(_work)); +} + +HC_SR04::~HC_SR04() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +HC_SR04::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (CDev::init() != OK) { + return ERROR; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + + if (_reports == nullptr) { + return ERROR; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); + } + + /* init echo port : */ + for (unsigned i = 0; i <= _sonars; i++) { + stm32_configgpio(_gpio_tab[i].trig_port); + stm32_gpiowrite(_gpio_tab[i].trig_port, false); + stm32_configgpio(_gpio_tab[i].echo_port); + _latest_sonar_measurements.push_back(0); + } + + usleep(200000); /* wait for 200ms; */ + + _cycling_rate = SR04_CONVERSION_INTERVAL; + + /* show the connected sonars in terminal */ + DEVICE_DEBUG("Number of sonars set: %d", _sonars); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + + return ret; +} + +int +HC_SR04::probe() +{ + return (OK); +} + +void +HC_SR04::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +HC_SR04::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +HC_SR04::get_minimum_distance() +{ + return _min_distance; +} + +float +HC_SR04::get_maximum_distance() +{ + return _max_distance; +} +void +HC_SR04::interrupt(unsigned time) +{ + if (_status == 0) { + _raising_time = time; + _status++; + return; + + } else if (_status == 1) { + _falling_time = time; + _status++; + return; + } + + return; +} + +int +HC_SR04::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) 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: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_cycling_rate); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_cycling_rate)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +HC_SR04::read(struct file *filp, char *buffer, size_t buflen) +{ + + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_cycling_rate * 2); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +HC_SR04::measure() +{ + + int ret; + /* + * Send a plus begin a measurement. + */ + stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true); + usleep(10); // 10us + stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false); + + stm32_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, sonar_isr); + _status = 0; + ret = OK; + + return ret; +} + +int +HC_SR04::collect() +{ + int ret = -EIO; +#if 0 + perf_begin(_sample_perf); + + /* read from the sensor */ + if (_status != 2) { + DEVICE_DEBUG("erro sonar %d ,status=%d", _cycle_counter, _status); + stm32_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, nullptr); + perf_end(_sample_perf); + return (ret); + } + + unsigned distance_time = _falling_time - _raising_time ; + + float si_units = (distance_time * 0.000170f) ; /* meter */ + struct distance_sensor_s report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (_sonars == 1) { + report.distance = si_units; + + for (unsigned i = 0; i < (SRF02_MAX_RANGEFINDERS); i++) { + report.id[i] = 0; + report.distance_vector[i] = 0; + } + + report.id[0] = SR04_ID_BASE; + report.distance_vector[0] = si_units; // 将测量值填入向量中，适应test()的要求 + report.just_updated = 1; + + } else { + /* for multiple sonars connected */ + + _latest_sonar_measurements[_cycle_counter] = si_units; + report.just_updated = 0; + + for (unsigned i = 0; i < SRF02_MAX_RANGEFINDERS; i++) { + if (i < _sonars) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + report.id[i] = SR04_ID_BASE + i; + report.just_updated++; + + } else { + report.distance_vector[i] = 0; + report.id[i] = 0; + } + + } + + report.distance = _latest_sonar_measurements[0]; // + } + + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it, if we are the primary */ + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + stm32_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, nullptr); /* close interrupt */ + perf_end(_sample_perf); +#endif + return ret; +} + +void +HC_SR04::start() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + measure(); /* begin measure */ + + /* schedule a cycle to start things */ + work_queue(HPWORK, + &_work, + (worker_t)&HC_SR04::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = nullptr; + + if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + + } +} + +void +HC_SR04::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +HC_SR04::cycle_trampoline(void *arg) +{ + + HC_SR04 *dev = (HC_SR04 *)arg; + + dev->cycle(); + +} + +void +HC_SR04::cycle() +{ + /*_circle_count 计录当前sonar　*/ + /* perform collection */ + if (OK != collect()) { + DEVICE_DEBUG("collection error"); + } + + /* change to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= _sonars) { + _cycle_counter = 0; + } + + /* 测量next sonar */ + if (OK != measure()) { + DEVICE_DEBUG("measure error sonar adress %d", _cycle_counter); + } + + + work_queue(HPWORK, + &_work, + (worker_t)&HC_SR04::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + +} + +void +HC_SR04::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); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace hc_sr04 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +HC_SR04 *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new HC_SR04(); + + 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(SR04_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, 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"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ +#if 0 + struct distance_sensor_s report; + ssize_t sz; + int ret; + + int fd = open(SR04_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'hc_sr04 start' if the driver is not running", SR04_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f of sonar %d,id=%d", (double)report.distance_vector[report.just_updated], report.just_updated, + report.id[report.just_updated]); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < SRF02_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u, id=%d", (double)report.distance_vector[count], count + 1, report.id[count]); + } + + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +#endif +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(SR04_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"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} /* namespace */ + +static int sonar_isr(int irq, void *context) +{ + unsigned time = hrt_absolute_time(); + /* ack the interrupts we just read */ + + if (hc_sr04::g_dev != nullptr) { + hc_sr04::g_dev->interrupt(time); + } + + return OK; +} + + + +int +hc_sr04_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + hc_sr04::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + hc_sr04::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + hc_sr04::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + hc_sr04::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + hc_sr04::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 022fe74be4..61345628b1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1050,6 +1050,18 @@ L3GD20::measure() report.z_raw = raw_report.z; +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + int16_t tx = -report.y_raw; + int16_t ty = -report.x_raw; + int16_t tz = -report.z_raw; + report.x_raw = tx; + report.y_raw = ty; + report.z_raw = tz; +#endif + + + + report.temperature_raw = raw_report.temp; float xraw_f = report.x_raw; @@ -1210,7 +1222,7 @@ start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { -#ifdef PX4_SPI_BUS_EXT +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO) g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index dc5ec44846..71796cfe8c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1577,6 +1577,15 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + int16_t tx = raw_accel_report.y; + int16_t ty = raw_accel_report.x; + int16_t tz = -raw_accel_report.z; + raw_accel_report.x = tx; + raw_accel_report.y = ty; + raw_accel_report.z = tz; +#endif + // use the temperature from the last mag reading accel_report.temperature = _last_temperature; @@ -1710,6 +1719,17 @@ LSM303D::mag_measure() mag_report.timestamp = hrt_absolute_time(); +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + int16_t tx = raw_mag_report.y; + int16_t ty = raw_mag_report.x; + int16_t tz = -raw_mag_report.z; + raw_mag_report.x = tx; + raw_mag_report.y = ty; + raw_mag_report.z = tz; +#endif + + + mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; @@ -1946,7 +1966,7 @@ start(bool external_bus, enum Rotation rotation, unsigned range) /* create the driver */ if (external_bus) { -#ifdef PX4_SPI_BUS_EXT +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); #else errx(0, "External SPI not available"); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index eaf68a3191..5559edf82e 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,8 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); diff --git a/src/drivers/mpu6500/CMakeLists.txt b/src/drivers/mpu6500/CMakeLists.txt new file mode 100644 index 0000000000..b1695c3f99 --- /dev/null +++ b/src/drivers/mpu6500/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__mpu6500 + MAIN mpu6500 + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + mpu6500.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp new file mode 100644 index 0000000000..f66f7ffd7a --- /dev/null +++ b/src/drivers/mpu6500/mpu6500.cpp @@ -0,0 +1,2412 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mpu6500.cpp + * + * Driver for the Invensense MPU6500 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6500_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6500_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6500_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6500_gyro_ext" + +// MPU 6000 registers +#define MPUREG_WHOAMI 0x75 +#define MPUREG_SMPLRT_DIV 0x19 +#define MPUREG_CONFIG 0x1A +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_ACCEL_CONFIG2 0x1D + +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_INT_STATUS 0x3A +#define MPUREG_ACCEL_XOUT_H 0x3B +#define MPUREG_ACCEL_XOUT_L 0x3C +#define MPUREG_ACCEL_YOUT_H 0x3D +#define MPUREG_ACCEL_YOUT_L 0x3E +#define MPUREG_ACCEL_ZOUT_H 0x3F +#define MPUREG_ACCEL_ZOUT_L 0x40 +#define MPUREG_TEMP_OUT_H 0x41 +#define MPUREG_TEMP_OUT_L 0x42 +#define MPUREG_GYRO_XOUT_H 0x43 +#define MPUREG_GYRO_XOUT_L 0x44 +#define MPUREG_GYRO_YOUT_H 0x45 +#define MPUREG_GYRO_YOUT_L 0x46 +#define MPUREG_GYRO_ZOUT_H 0x47 +#define MPUREG_GYRO_ZOUT_L 0x48 +#define MPUREG_USER_CTRL 0x6A +#define MPUREG_PWR_MGMT_1 0x6B +#define MPUREG_PWR_MGMT_2 0x6C +#define MPUREG_FIFO_COUNTH 0x72 +#define MPUREG_FIFO_COUNTL 0x73 +#define MPUREG_FIFO_R_W 0x74 +//#define MPUREG_PRODUCT_ID 0x0C +//#define MPUREG_TRIM1 0x0D +//#define MPUREG_TRIM2 0x0E +//#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 +#define MPUREG_GYRO_SELFTEST_X 0x00 +#define MPUREG_GYRO_SELFTEST_Y 0x01 +#define MPUREG_GYRO_SELFTEST_Z 0x02 +#define MPUREG_ACCEL_SELFTEST_X 0x0D +#define MPUREG_ACCEL_SELFTEST_Y 0x0E +#define MPUREG_ACCEL_SELFTEST_Z 0x0F + + +// Configuration bits MPU 3000 and MPU 6000 (not revised)? +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_INT_STATUS_DATA 0x01 + + +#define MPU6500_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6500_ACCEL_DEFAULT_RATE 1000 +#define MPU6500_ACCEL_MAX_OUTPUT_RATE 280 +#define MPU6500_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6500_GYRO_DEFAULT_RANGE_G 8 +#define MPU6500_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU6500_GYRO_MAX_OUTPUT_RATE MPU6500_ACCEL_MAX_OUTPUT_RATE +#define MPU6500_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6500_DEFAULT_ONCHIP_FILTER_FREQ 42 + +#define MPU6500_ONE_G 9.80665f + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + +/* + the MPU6500 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6500_LOW_BUS_SPEED 1000*1000 +#define MPU6500_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ + +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates by comparing + accelerometer values. This time reduction is enough to cope with + worst case timing jitter due to other timers + */ +#define MPU6500_TIMER_REDUCTION 200 + +class MPU6500_gyro; + +class MPU6500 : public device::SPI +{ +public: + MPU6500(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + virtual ~MPU6500(); + + 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(); + + void print_registers(); + + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + + // deliberately cause a sensor error + void test_error(); + +protected: + virtual int probe(); + + friend class MPU6500_gyro; + + virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); + virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + MPU6500_gyro *_gyro; + uint8_t _product; /** product code */ + + struct hrt_call _call; + unsigned _call_interval; + + ringbuffer::RingBuffer *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + ringbuffer::RingBuffer *_gyro_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + + unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; + perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _good_transfers; + perf_counter_t _reset_retries; + perf_counter_t _duplicates; + perf_counter_t _system_latency_perf; + perf_counter_t _controller_latency_perf; + + uint8_t _register_wait; + uint64_t _reset_wait; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + + Integrator _accel_int; + Integrator _gyro_int; + + enum Rotation _rotation; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU6500_NUM_CHECKED_REGISTERS 9 + static const uint8_t _checked_registers[MPU6500_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU6500_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + + // last temperature reading for print_info() + float _last_temperature; + + // keep last accel reading for duplicate detection + uint16_t _last_accel[3]; + bool _got_duplicate; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + /** + * 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); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Read a register from the MPU6500 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg, uint32_t speed = MPU6500_LOW_BUS_SPEED); + uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the MPU6500 + * + * @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 MPU6500 + * + * 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); + + /** + * Write a register in the MPU6500, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the MPU6500 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Swap a 16-bit value read from the MPU6500 to native byte order. + */ + uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(unsigned desired_sample_rate_hz); + + /* + check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + MPU6500(const MPU6500 &); + MPU6500 operator=(const MPU6500 &); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6500, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) +}; + +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU6500::_checked_registers[MPU6500_NUM_CHECKED_REGISTERS] = { + MPUREG_PWR_MGMT_1, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG +}; + + + +/** + * Helper class implementing the gyro driver node. + */ +class MPU6500_gyro : public device::CDev +{ +public: + MPU6500_gyro(MPU6500 *parent, const char *path); + ~MPU6500_gyro(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class MPU6500; + + void parent_poll_notify(); + +private: + MPU6500 *_parent; + orb_advert_t _gyro_topic; + int _gyro_orb_class_instance; + int _gyro_class_instance; + + /* do not allow to copy this class due to pointer data members */ + MPU6500_gyro(const MPU6500_gyro &); + MPU6500_gyro operator=(const MPU6500_gyro &); +}; + +/** driver 'main' command */ +extern "C" { __EXPORT int mpu6500_main(int argc, char *argv[]); } + +MPU6500::MPU6500(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : + SPI("MPU6500", path_accel, bus, device, SPIDEV_MODE3, MPU6500_LOW_BUS_SPEED), + _gyro(new MPU6500_gyro(this, path_gyro)), + _product(0), + _call{}, + _call_interval(0), + _accel_reports(nullptr), + _accel_scale{}, + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _gyro_reports(nullptr), + _gyro_scale{}, + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6500_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6500_gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "mpu6500_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6500_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "mpu6500_bad_registers")), + _good_transfers(perf_alloc(PC_COUNT, "mpu6500_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu6500_reset_retries")), + _duplicates(perf_alloc(PC_COUNT, "mpu6500_duplicates")), + _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _register_wait(0), + _reset_wait(0), + _accel_filter_x(MPU6500_ACCEL_DEFAULT_RATE, MPU6500_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU6500_ACCEL_DEFAULT_RATE, MPU6500_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU6500_ACCEL_DEFAULT_RATE, MPU6500_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU6500_GYRO_DEFAULT_RATE, MPU6500_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU6500_GYRO_DEFAULT_RATE, MPU6500_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU6500_GYRO_DEFAULT_RATE, MPU6500_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU6500_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU6500_GYRO_MAX_OUTPUT_RATE, true), + _rotation(rotation), + _checked_next(0), + _in_factory_test(false), + _last_temperature(0), + _last_accel{}, + _got_duplicate(false) +{ + // disable debug() calls + _debug_enabled = false; + + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6500; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6500; + + // default accel 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; + + // default gyro scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + memset(&_call, 0, sizeof(_call)); +} + +MPU6500::~MPU6500() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the gyro subdriver */ + delete _gyro; + + /* free any existing reports */ + if (_accel_reports != nullptr) { + delete _accel_reports; + } + + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); +} + +int +MPU6500::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + + if (_accel_reports == nullptr) { + goto out; + } + + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + + if (_gyro_reports == nullptr) { + goto out; + } + + if (reset() != OK) { + goto out; + } + + /* Initialize offsets and scales */ + _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; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + + /* do CDev init for the gyro device node, keep it optional */ + ret = _gyro->init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("gyro init failed"); + return ret; + } + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + + if (_accel_topic == nullptr) { + warnx("ADVERT FAIL"); + } + + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + + if (_gyro->_gyro_topic == nullptr) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + +int MPU6500::reset() +{ + // if the mpu6500 is initialised after the l3gd20 and lsm303d + // then if we don't do an irqsave/irqrestore here the mpu6500 + // frequenctly comes up in a bad state where all transfers + // come as zero + uint8_t tries = 5; + + while (--tries != 0) { + irqstate_t state; + state = irqsave(); + + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + // Wake up device and select GyroZ clock. Note that the + // MPU6500 starts up in sleep mode, and it can take some time + // for it to come out of sleep + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); + + if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { + break; + } + + perf_count(_reset_retries); + usleep(2000); + } + + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { + return -EIO; + } + + usleep(1000); + + // SAMPLE RATE + _set_sample_rate(_sample_rate); + usleep(1000); + + // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) + // was 90 Hz, but this ruins quality and does not improve the + // system response + _set_dlpf_filter(MPU6500_DEFAULT_ONCHIP_FILTER_FREQ); + usleep(1000); + // Gyro scale 2000 deg/s () + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + usleep(1000); + + // correct gyro scale factors + // scale to rad/s in SI units + // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s + // scaling factor: + // 1/(2^15)*(2000/180)*PI + _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; + + set_accel_range(8); + + usleep(1000); + + // INT CFG => Interrupt on Data Ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + usleep(1000); + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + usleep(1000); + + // Oscillator set + // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); + usleep(1000); + + return OK; +} + +int +MPU6500::probe() +{ + + /* look for a product ID we recognise */ + uint8_t who = read_reg(MPUREG_WHOAMI); + +// log("WHOAMI 0x%02x", who); + if (who != 0x70) {return -EIO;} + + return (OK); +} + +/* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro +*/ +void +MPU6500::_set_sample_rate(unsigned desired_sample_rate_hz) +{ + if (desired_sample_rate_hz == 0 || + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz = MPU6500_GYRO_DEFAULT_RATE; + } + + uint8_t div = 1000 / desired_sample_rate_hz; + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); + _sample_rate = 1000 / div; +} + +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +MPU6500::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz == 0) { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + + } else if (frequency_hz <= 5) { + filter = BITS_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 10) { + filter = BITS_DLPF_CFG_10HZ; + + } else if (frequency_hz <= 20) { + filter = BITS_DLPF_CFG_20HZ; + + } else if (frequency_hz <= 42) { + filter = BITS_DLPF_CFG_42HZ; + + } else if (frequency_hz <= 98) { + filter = BITS_DLPF_CFG_98HZ; + + } else if (frequency_hz <= 188) { + filter = BITS_DLPF_CFG_188HZ; + + } else if (frequency_hz <= 256) { + filter = BITS_DLPF_CFG_256HZ_NOLPF2; + + } else { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } + + write_checked_reg(MPUREG_CONFIG, filter); +} + +ssize_t +MPU6500::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(accel_report); + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) { + return -EAGAIN; + } + + perf_count(_accel_reads); + + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast(buffer); + int transferred = 0; + + while (count--) { + if (!_accel_reports->get(arp)) { + break; + } + + transferred++; + arp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); +} + +int +MPU6500::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +int +MPU6500::accel_self_test() +{ + if (self_test()) { + return 1; + } + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.y_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } + + return 0; +} + +int +MPU6500::gyro_self_test() +{ + if (self_test()) { + return 1; + } + + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) { + return 1; + } + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { + return 1; + } + + if (fabsf(_gyro_scale.y_offset) > max_offset) { + return 1; + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { + return 1; + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { + return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + + return 0; +} + + + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values (as per datasheet) + */ +int +MPU6500::factory_self_test() +{ +// _in_factory_test = true; +// uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); +// uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); +// const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS +// write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + +// struct MPUReport mpu_report; +// float accel_baseline[3]; +// float gyro_baseline[3]; +// float accel[3]; +// float gyro[3]; +// float accel_ftrim[3]; +// float gyro_ftrim[3]; +// +// // get baseline values without self-test enabled +// set_frequency(MPU6500_HIGH_BUS_SPEED); + +// memset(accel_baseline, 0, sizeof(accel_baseline)); +// memset(gyro_baseline, 0, sizeof(gyro_baseline)); +// memset(accel, 0, sizeof(accel)); +// memset(gyro, 0, sizeof(gyro)); +// +// for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); +// atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); +// atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); +// gtrim[0] = trims[0] & 0x1F; +// gtrim[1] = trims[1] & 0x1F; +// gtrim[2] = trims[2] & 0x1F; +// + // convert factory trims to right units +// for (uint8_t i=0; i<3; i++) { +// accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); +// gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); +// } + // Y gyro trim is negative +// gyro_ftrim[1] *= -1; + +// for (uint8_t i=0; i<3; i++) { +// float diff = accel[i]-accel_baseline[i]; +// float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; +// ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", +// (unsigned)i, +// (int)(1000*accel_baseline[i]), +// (int)(1000*accel[i]), +// (int)(1000*diff), +// (int)(1000*accel_ftrim[i]), +// (int)err); +// if (fabsf(err) > 14) { +// ::printf("FAIL\n"); +// ret = -EIO; +// } +// } +// for (uint8_t i=0; i<3; i++) { +// float diff = gyro[i]-gyro_baseline[i]; +// float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; +// ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", +// (unsigned)i, +// (int)(1000*gyro_baseline[i]), +// (int)(1000*gyro[i]), +// (int)(1000*(gyro[i]-gyro_baseline[i])), +// (int)(1000*gyro_ftrim[i]), +// (int)err); +// if (fabsf(err) > 14) { +// ::printf("FAIL\n"); +// ret = -EIO; +// } +// } + +// write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); +// write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); +// +// _in_factory_test = false; +// if (ret == OK) { +// ::printf("PASSED\n"); +// } + + return ret; +} + + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU6500::test_error() +{ + _in_factory_test = true; + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); + _in_factory_test = false; +} + +ssize_t +MPU6500::gyro_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(gyro_report); + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) { + return -EAGAIN; + } + + perf_count(_gyro_reads); + + /* copy reports out of our buffer to the caller */ + gyro_report *grp = reinterpret_cast(buffer); + int transferred = 0; + + while (count--) { + if (!_gyro_reports->get(grp)) { + break; + } + + transferred++; + grp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); +} + +int +MPU6500::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCRESET: + return reset(); + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_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, 1000); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU6500_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) { + return -EINVAL; + } + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f / ticks; + _set_dlpf_filter(cutoff_freq_hz); + _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); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_interval = ticks; + + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6500 clock + */ + _call.period = _call_interval - MPU6500_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case ACCELIOCGSAMPLERATE: + return _sample_rate; + + case ACCELIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + 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; + + case ACCELIOCSRANGE: + return set_accel_range(arg); + + case ACCELIOCGRANGE: + return (unsigned long)((_accel_range_m_s2) / MPU6500_ONE_G + 0.5f); + + case ACCELIOCSELFTEST: + return accel_self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +MPU6500::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _gyro_reports->size(); + + case GYROIOCGSAMPLERATE: + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case GYROIOCGLOWPASS: + return _gyro_filter_x.get_cutoff_freq(); + + case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_rad_s = xx + return -EINVAL; + + case GYROIOCGRANGE: + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); + + case GYROIOCSELFTEST: + return gyro_self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +MPU6500::read_reg(unsigned reg, uint32_t speed) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + // general register transfer at low clock speed + set_frequency(speed); + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +MPU6500::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + // general register transfer at low clock speed + set_frequency(MPU6500_LOW_BUS_SPEED); + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +MPU6500::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + // general register transfer at low clock speed + set_frequency(MPU6500_LOW_BUS_SPEED); + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +MPU6500::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); +} + +void +MPU6500::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < MPU6500_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + +int +MPU6500::set_accel_range(unsigned max_g_in) +{ + // workaround for bugged versions of MPU6k (rev C) +// switch (_product) { +// case MPU6500ES_REV_C4: +// case MPU6500ES_REV_C5: +// case MPU6500_REV_C4: +// case MPU6500_REV_C5: +// write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); +// _accel_range_scale = (MPU6500_ONE_G / 4096.0f); +// _accel_range_m_s2 = 8.0f * MPU6500_ONE_G; +// return OK; +// } + + uint8_t afs_sel; + float lsb_per_g; + float max_accel_g; + + if (max_g_in > 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; + } + + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (MPU6500_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * MPU6500_ONE_G; + + return OK; +} + +void +MPU6500::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - MPU6500_TIMER_REDUCTION, + (hrt_callout)&MPU6500::measure_trampoline, this); +} + +void +MPU6500::stop() +{ + hrt_cancel(&_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); +} + +void +MPU6500::measure_trampoline(void *arg) +{ + MPU6500 *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +MPU6500::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next], MPU6500_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu6500 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % MPU6500_NUM_CHECKED_REGISTERS; +} + +void +MPU6500::measure() +{ + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct MPUReport mpu_report; + + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the MPU6500 in one pass. + */ + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6500_HIGH_BUS_SPEED); + + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { + return; + } + + // check_registers(); + /* + see if this is duplicate accelerometer data. Note that we + can't use the data ready interrupt status bit in the status + register as that also goes high on new gyro data, and when + we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being + sampled at 8kHz, so we would incorrectly think we have new + data when we are in fact getting duplicate accelerometer data. + */ + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + // it isn't new data - wait for next timer + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); + _got_duplicate = false; + + /* + * Convert from big to little endian + */ + + report.accel_x = int16_t_from_bytes(mpu_report.accel_x); + report.accel_y = int16_t_from_bytes(mpu_report.accel_y); + report.accel_z = int16_t_from_bytes(mpu_report.accel_z); + + report.temp = int16_t_from_bytes(mpu_report.temp); + + report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); + report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); + report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + + + + + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } + +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + /* + * Swap axes and negate z + */ + int16_t accel_xt = report.accel_y; + int16_t accel_yt = report.accel_x; + int16_t accel_zt = ((report.accel_z == -32768) ? 32767 : -report.accel_z); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = report.gyro_x; + int16_t gyro_zt = ((report.gyro_z == -32768) ? 32767 : -report.gyro_z); + + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.accel_z = accel_zt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; + report.gyro_z = gyro_zt; + +#else + /* + * Swap axes and negate y + */ + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; +#endif + /* + * Report buffers. + */ + accel_report arb; + gyro_report grb; + + /* + * Adjust and scale results to m/s^2. + */ + grb.timestamp = arb.timestamp = hrt_absolute_time(); + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 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. + */ + + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + _last_temperature = (report.temp) / 361.0f + 35.0f; + + arb.temperature_raw = report.temp; + arb.temperature = _last_temperature; + + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; + + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; + + grb.temperature_raw = report.temp; + grb.temperature = _last_temperature; + + _accel_reports->force(&arb); + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + if (accel_notify) { + poll_notify(POLLIN); + } + + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + perf_begin(_system_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + + if (gyro_notify && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +MPU6500::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); + perf_print_counter(_duplicates); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < MPU6500_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i], MPU6500_HIGH_BUS_SPEED); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } + + ::printf("temperature: %.1f\n", (double)_last_temperature); +} + +void +MPU6500::print_registers() +{ + printf("MPU6500 registers\n"); + + for (uint8_t reg = 0; reg <= 108; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ", (unsigned)reg, (unsigned)v); + + if (reg % 16 == 0) { + printf("\n"); + } + } + + printf("\n"); +} + + +MPU6500_gyro::MPU6500_gyro(MPU6500 *parent, const char *path) : + CDev("MPU6500_gyro", path), + _parent(parent), + _gyro_topic(nullptr), + _gyro_orb_class_instance(-1), + _gyro_class_instance(-1) +{ +} + +MPU6500_gyro::~MPU6500_gyro() +{ + if (_gyro_class_instance != -1) { + unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } +} + +int +MPU6500_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); + + return ret; +} + +void +MPU6500_gyro::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +MPU6500_gyro::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->gyro_read(filp, buffer, buflen); +} + +int +MPU6500_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } +} + +/** + * Local functions in support of the shell command. + */ +namespace mpu6500 +{ + + +MPU6500 *g_dev_int; // on internal bus +MPU6500 *g_dev_ext; // on external bus + +void start(bool, enum Rotation, int range); +void stop(bool); +void test(bool); +void reset(bool); +void info(bool); +void regdump(bool); +void testerror(bool); +void factorytest(bool); +void usage(); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation, int range) +{ + int fd; + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; + + if (*g_dev_ptr != nullptr) + /* if already started, the still command succeeded */ + { + errx(0, "already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) + *g_dev_ptr = new MPU6500(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_ptr = new MPU6500(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); + } + + if (*g_dev_ptr == nullptr) { + goto fail; + } + + if (OK != (*g_dev_ptr)->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(path_accel, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { + goto fail; + } + + close(fd); + + exit(0); +fail: + + if (*g_dev_ptr != nullptr) { + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; + } + + errx(1, "driver start failed"); +} + +void +stop(bool external_bus) +{ + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr != nullptr) { + delete *g_dev_ptr; + *g_dev_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test(bool external_bus) +{ + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; + accel_report a_report; + gyro_report g_report; + ssize_t sz; + + /* get the driver */ + int fd = open(path_accel, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mpu6500 start')", + path_accel); + + /* get the driver */ + int fd_gyro = open(path_gyro, O_RDONLY); + + if (fd_gyro < 0) { + err(1, "%s open failed", path_gyro); + } + + /* reset to manual polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + err(1, "reset to manual polling"); + } + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(a_report)); + err(1, "immediate acc read failed"); + } + + warnx("single read"); + warnx("time: %lld", a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / MPU6500_ONE_G)); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(g_report)); + err(1, "immediate gyro read failed"); + } + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + /* reset to default polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "reset to default polling"); + } + + close(fd); + close(fd_gyro); + + /* XXX add poll-rate tests here too */ + + reset(external_bus); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset(bool external_bus) +{ + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + int fd = open(path_accel, 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"); + } + + close(fd); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info(bool external_bus) +{ + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); + + exit(0); +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus) +{ + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + exit(0); +} + +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus) +{ + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + (*g_dev_ptr)->test_error(); + + exit(0); +} + +/** + * Dump the register information + */ +void +factorytest(bool external_bus) +{ + MPU6500 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); + warnx("options:"); + warnx(" -X (external bus)"); + warnx(" -R rotation"); + warnx(" -a accel range (in g)"); +} + +} // namespace + +int +mpu6500_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + + case 'a': + accel_range = atoi(optarg); + break; + + default: + mpu6500::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + + */ + if (!strcmp(verb, "start")) { + mpu6500::start(external_bus, rotation, accel_range); + } + + if (!strcmp(verb, "stop")) { + mpu6500::stop(external_bus); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + mpu6500::test(external_bus); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + mpu6500::reset(external_bus); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + mpu6500::info(external_bus); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + mpu6500::regdump(external_bus); + } + + if (!strcmp(verb, "factorytest")) { + mpu6500::factorytest(external_bus); + } + + if (!strcmp(verb, "testerror")) { + mpu6500::testerror(external_bus); + } + + mpu6500::usage(); + exit(1); +} diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 244d8c4d45..6d89ce2221 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -321,6 +321,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_MINDPX_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}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, + {GPIO_GPIO12_INPUT, GPIO_GPIO12_OUTPUT, 0}, + {0, 0, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -1859,7 +1875,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) case 6: set_mode(MODE_6PWM); @@ -2181,6 +2198,89 @@ PX4FMU::sensor_reset(int ms) // stm32_configgpio(GPIO_ACCEL_DRDY); // stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif +#endif + +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + // stm32_configgpio(GPIO_SPI_CS_FRAM_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); + + stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + // stm32_gpiowrite(GPIO_SPI_CS_FRAM_OFF,0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); + + stm32_configgpio(GPIO_SPI4_SCK_OFF); + stm32_configgpio(GPIO_SPI4_MISO_OFF); + stm32_configgpio(GPIO_SPI4_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); + + stm32_configgpio(GPIO_GYRO_DRDY_OFF); + stm32_configgpio(GPIO_MAG_DRDY_OFF); + stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); + + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); + + // /* set the sensor rail off */ + // stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + // + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + // + // /* re-enable power */ + // + // /* switch the sensor rail back on */ + // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + // + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + // stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* 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); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_SPI4_SCK); + stm32_configgpio(GPIO_SPI4_MISO); + stm32_configgpio(GPIO_SPI4_MOSI); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + #endif #endif } @@ -2232,6 +2332,13 @@ PX4FMU::peripheral_reset(int ms) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); #endif +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + + if (ms < 1) { + ms = 10; + } + +#endif } void @@ -2574,7 +2681,8 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) servo_mode = PX4FMU::MODE_6PWM; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -2582,7 +2690,8 @@ fmu_new_mode(PortMode new_mode) #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) case PORT_PWM4: /* select 4-pin PWM mode */ @@ -3015,7 +3124,8 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -3106,7 +3216,8 @@ fmu_main(int argc, char *argv[]) #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, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); diff --git a/src/drivers/rgbled_pwm/CMakeLists.txt b/src/drivers/rgbled_pwm/CMakeLists.txt new file mode 100644 index 0000000000..7bc768d91b --- /dev/null +++ b/src/drivers/rgbled_pwm/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__rgbled_pwm + MAIN rgbled + COMPILE_FLAGS + -Os + SRCS + drv_led_pwm.cpp + rgbled_pwm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/rgbled_pwm/drv_led_pwm.cpp b/src/drivers/rgbled_pwm/drv_led_pwm.cpp new file mode 100644 index 0000000000..02bc42336d --- /dev/null +++ b/src/drivers/rgbled_pwm/drv_led_pwm.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file drv_led_pwm.cpp +* +* +*/ + +#include + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include +#include + + +#include + + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(led_pwm_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) + +static void led_pwm_timer_init(unsigned timer); +static void led_pwm_timer_set_rate(unsigned timer, unsigned rate); +static void led_pwm_channel_init(unsigned channel); + +int led_pwm_servo_set(unsigned channel, uint8_t value); +unsigned led_pwm_servo_get(unsigned channel); +int led_pwm_servo_init(void); +void led_pwm_servo_deinit(void); +void led_pwm_servo_arm(bool armed); +unsigned led_pwm_timer_get_period(unsigned timer); + + +const struct io_timers_t led_pwm_timers[3] = { + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + .vectorno = STM32_IRQ_TIM3, + .first_channel_index = 4, + .last_channel_index = 7, + .handler = io_timer_handler0, + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + .vectorno = STM32_IRQ_TIM3, + .first_channel_index = 4, + .last_channel_index = 7, + .handler = io_timer_handler1, + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN, + .vectorno = STM32_IRQ_TIM3, + .first_channel_index = 4, + .last_channel_index = 7, + .handler = io_timer_handler2, + } +}; + +#define LED_TIM3_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0) +#define LED_TIM3_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7) +#define LED_TIM3_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1) +#define LED_TIM3_CH3IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0) +#define LED_TIM3_CH2IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7) +#define LED_TIM3_CH4IN (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1) + +const struct timer_io_channels_t led_pwm_channels[3] = { + { + .gpio_out = LED_TIM3_CH3OUT, + .gpio_in = LED_TIM3_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + }, + { + .gpio_out = LED_TIM3_CH2OUT, + .gpio_in = LED_TIM3_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + }, + { + .gpio_out = LED_TIM3_CH4OUT, + .gpio_in = LED_TIM3_CH4IN, + .timer_index = 2, + .timer_channel = 4, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + } +}; + + +static void +led_pwm_timer_init(unsigned timer) +{ + /* enable the timer clock before we try to talk to it */ + modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) { + /* master output enable = on */ + rBDTR(timer) = ATIM_BDTR_MOE; + } + + /* configure the timer to free-run at 1MHz */ + rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1; + + /* default to updating at 50Hz */ + led_pwm_timer_set_rate(timer, 50); + + /* note that the timer is left disabled - arming is performed separately */ +} +unsigned +led_pwm_timer_get_period(unsigned timer) +{ + return (rARR(timer)); +} +static void +led_pwm_timer_set_rate(unsigned timer, unsigned rate) +{ + /* configure the timer to update at the desired rate */ + rARR(timer) = 1000000 / rate; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; +} + + +static void +led_pwm_channel_init(unsigned channel) +{ + unsigned timer = led_pwm_channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(led_pwm_channels[channel].gpio_out); + + /* configure the channel */ + switch (led_pwm_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + //rCCR1(timer) = led_pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + //rCCR2(timer) = led_pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + //rCCR3(timer) = led_pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + //rCCR4(timer) = led_pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC4E; + break; + } +} + +int +led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= 3) { + return -1; + } + + unsigned timer = led_pwm_channels[channel].timer_index; + + /* test timer for validity */ + if ((led_pwm_timers[timer].base == 0) || + (led_pwm_channels[channel].gpio_out== 0)) { + return -1; + } + + unsigned period = led_pwm_timer_get_period(timer); + + unsigned value = period - (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + + switch (led_pwm_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} +unsigned +led_pwm_servo_get(unsigned channel) +{ + if (channel >= 3) { + return 0; + } + + unsigned timer = led_pwm_channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((led_pwm_timers[timer].base == 0) || + (led_pwm_channels[channel].timer_channel == 0)) { + return 0; + } + + /* configure the channel */ + switch (led_pwm_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + unsigned period = led_pwm_timer_get_period(timer); + return ((value + 1) * 255 / period); +} +int +led_pwm_servo_init(void) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < 3; i++) { + led_pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < 3; i++) { + led_pwm_channel_init(i); + } + + led_pwm_servo_arm(true); + return OK; +} + +void +led_pwm_servo_deinit(void) +{ + /* disable the timers */ + led_pwm_servo_arm(false); +} +void +led_pwm_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < 3; i++) { + if (led_pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; + } + } + } +} + diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp new file mode 100644 index 0000000000..cf2a91b91c --- /dev/null +++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp @@ -0,0 +1,697 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rgbled_pwm.cpp + * + * Driver for the onboard RGB LED controller by PWM. + * this driver is based the PX4 led driver + * + */ + +#include + +//#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 + +class RGBLED_PWM : public device::CDev +{ +public: + RGBLED_PWM(); + virtual ~RGBLED_PWM(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + struct hrt_call _call; + struct hrt_call _call_r; + struct hrt_call _call_g; + struct hrt_call _call_b; + + work_s _work; + + rgbled_mode_t _mode; + rgbled_pattern_t _pattern; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + bool _enable; + float _brightness; + + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + static void pwm_begin(void *arg); + static void pwm_end_r(void *arg); + static void pwm_end_g(void *arg); + static void pwm_end_b(void *arg); + + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); +extern int led_pwm_servo_set(unsigned channel, uint8_t value); +extern unsigned led_pwm_servo_get(unsigned channel); +extern int led_pwm_servo_init(void); +extern void led_pwm_servo_deinit(void); + + +/* for now, we only support one RGBLED */ +namespace +{ +RGBLED_PWM *g_rgbled = nullptr; +} + +void rgbled_usage(); + +RGBLED_PWM::RGBLED_PWM() : + CDev("rgbled", RGBLED0_DEVICE_PATH), + _call{}, + _call_r{}, + _call_g{}, + _call_b{}, + _mode(RGBLED_MODE_OFF), + _r(0), + _g(0), + _b(0), + _enable(false), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); + + memset(&_call, 0, sizeof(_call)); + memset(&_call_r, 0, sizeof(_call_r)); + memset(&_call_g, 0, sizeof(_call_g)); + memset(&_call_b, 0, sizeof(_call_b)); +} + +RGBLED_PWM::~RGBLED_PWM() +{ +} + +int +RGBLED_PWM::init() +{ + /* switch off LED on start */ + CDev::init(); +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + led_pwm_servo_init(); + send_led_enable(false); + send_led_rgb(); +#endif + return OK; +} +void +RGBLED_PWM::pwm_begin(void *arg) +{ +} + + +void +RGBLED_PWM::pwm_end_r(void *arg) +{ +} +void +RGBLED_PWM::pwm_end_g(void *arg) +{ +} +void +RGBLED_PWM::pwm_end_b(void *arg) +{ +} + + +int +RGBLED_PWM::info() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + ret = get(on, powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + DEVICE_LOG("state: %s", on ? "ON" : "OFF"); + DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + + } else { + warnx("failed to read led"); + } + + return ret; +} +int +RGBLED_PWM::probe() +{ + return (OK); +} +int +RGBLED_PWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case RGBLED_SET_RGB: + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); + return OK; + + case RGBLED_SET_COLOR: + /* set the specified color name */ + set_color((rgbled_color_t)arg); + send_led_rgb(); + return OK; + + case RGBLED_SET_MODE: + /* set the specified mode */ + set_mode((rgbled_mode_t)arg); + return OK; + + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t *)arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +RGBLED_PWM::led_trampoline(void *arg) +{ + RGBLED_PWM *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLED_PWM::led() +{ + if (!_should_run) { + _running = false; + return; + } + + switch (_mode) { + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) { + _counter = 0; + } + + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) { + _counter = 0; + } + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); +// send_led_rgb(); + break; + + case RGBLED_MODE_PATTERN: + + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) { + _counter = 0; + } + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; + } + + _counter++; + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, _led_interval); +} + +/** + * Parse color constant and set _r _g _b values + */ +void +RGBLED_PWM::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + warnx("color unknown"); + break; + } +} + +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ +void +RGBLED_PWM::set_mode(rgbled_mode_t mode) +{ + if (mode != _mode) { + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + _should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + _should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + warnx("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 1); + } + + } +} + +/** + * Set pattern for PATTERN mode, but don't change current mode + */ +void +RGBLED_PWM::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +RGBLED_PWM::send_led_enable(bool enable) +{ + _enable = enable; + send_led_rgb(); + return (OK); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED_PWM::send_led_rgb() +{ +#if defined(CONFIG_ARCH_BOARD_MINDPX_V2) + + if (_enable) { + led_pwm_servo_set(0, _r); + led_pwm_servo_set(1, _g); + led_pwm_servo_set(2, _b); + + } else { + led_pwm_servo_set(0, 0); + led_pwm_servo_set(1, 0); + led_pwm_servo_set(2, 0); + } + +#endif + return (OK); +} + +int +RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + powersave = OK; on = _enable; + r = _r; + g = _g; + b = _b; + return OK; +} + +void +rgbled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); +} + +int +rgbled_main(int argc, char *argv[]) +{ + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + break; + + case 'b': + break; + + default: + rgbled_usage(); + exit(0); + } + } + + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + errx(1, "already started"); + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED_PWM(); + + if (g_rgbled == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + warnx("not started"); + rgbled_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + } + + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; + + ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + fd = open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + } + + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + close(fd); + + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } + + exit(ret); + } + + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: rgbled rgb "); + } + + fd = open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + } + + rgbled_rgbset_t v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + close(fd); + exit(ret); + } + + rgbled_usage(); + exit(0); +} diff --git a/src/drivers/srf02_i2c/CMakeLists.txt b/src/drivers/srf02_i2c/CMakeLists.txt new file mode 100644 index 0000000000..5be082da58 --- /dev/null +++ b/src/drivers/srf02_i2c/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__srf02_i2c + MAIN srf02_i2c + COMPILE_FLAGS + -Os + SRCS + srf02_i2c.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/srf02_i2c/srf02_i2c.cpp b/src/drivers/srf02_i2c/srf02_i2c.cpp new file mode 100644 index 0000000000..65c22629ed --- /dev/null +++ b/src/drivers/srf02_i2c/srf02_i2c.cpp @@ -0,0 +1,961 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file srf02_i2c.cpp + * @author Greg Hulands + * @author Jon Verbeke + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +/* Configuration Constants */ +#define SRF02_I2C_BUS PX4_I2C_BUS_EXPANSION +#define SRF02_I2C_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define SRF02_DEVICE_PATH "/dev/srf02" + +/* MB12xx Registers addresses */ + +#define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ +#define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define SRF02_MIN_DISTANCE (0.20f) +#define SRF02_MAX_DISTANCE (6.00f) + +#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class SRF02_I2C : public device::I2C +{ +public: + SRF02_I2C(int bus = SRF02_I2C_BUS, int address = SRF02_I2C_BASEADDR); + virtual ~SRF02_I2C(); + + 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(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + + /** + * 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. + * + * @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. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults SRF02_MIN_DISTANCE + * and SRF02_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" { __EXPORT int srf02_i2c_main(int argc, char *argv[]);} + +SRF02_I2C::SRF02_I2C(int bus, int address) : + I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000), + _min_distance(SRF02_MIN_DISTANCE), + _max_distance(SRF02_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "srf02_i2c_read")), + _comms_errors(perf_alloc(PC_COUNT, "srf02_i2c_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "srf02_i2c_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + +{ + /* enable debug() calls */ + _debug_enabled = false; + + /* work_cancel in the dtor will explode if we don't do this... */ + memset(&_work, 0, sizeof(_work)); +} + +SRF02_I2C::~SRF02_I2C() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +SRF02_I2C::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + return ret; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + + _index_counter = SRF02_I2C_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + + if (_reports == nullptr) { + return ret; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); + } + + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = SRF02_I2C_BASEADDR + counter * 2; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + DEVICE_DEBUG("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = SRF02_I2C_BASEADDR; + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = SRF02_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size()); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + + return ret; +} + +int +SRF02_I2C::probe() +{ + return measure(); +} + +void +SRF02_I2C::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SRF02_I2C::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SRF02_I2C::get_minimum_distance() +{ + return _min_distance; +} + +float +SRF02_I2C::get_maximum_distance() +{ + return _max_distance; +} + +int +SRF02_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) 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: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_cycling_rate); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_cycling_rate)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +SRF02_I2C::read(struct file *filp, char *buffer, size_t buflen) +{ + + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_cycling_rate * 2); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +SRF02_I2C::measure() +{ + + int ret; + + /* + * Send the command to begin a measurement. + */ + + uint8_t cmd[2]; + cmd[0] = 0x00; + cmd[1] = SRF02_TAKE_RANGE_REG; + ret = transfer(cmd, 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SRF02_I2C::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + uint8_t cmd = 0x02; + perf_begin(_sample_perf); + + ret = transfer(&cmd, 1, nullptr, 0); + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + DEVICE_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; + + /* publish it, if we are the primary */ + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SRF02_I2C::start() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&SRF02_I2C::cycle_trampoline, this, 5); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = nullptr; + + if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + + } +} + +void +SRF02_I2C::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SRF02_I2C::cycle_trampoline(void *arg) +{ + + SRF02_I2C *dev = (SRF02_I2C *)arg; + + dev->cycle(); + +} + +void +SRF02_I2C::cycle() +{ + if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + set_address(_index_counter); + + /* perform collection */ + if (OK != collect()) { + DEVICE_DEBUG("collection error"); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02_I2C::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_cycling_rate)); + return; + } + } + + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ + if (OK != measure()) { + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02_I2C::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + +} + +void +SRF02_I2C::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); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace srf02_i2c +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SRF02_I2C *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SRF02_I2C(SRF02_I2C_BUS); + + 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(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, 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"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct distance_sensor_s report; + ssize_t sz; + int ret; + + int fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'srf02_i2c start' if the driver is not running", SRF02_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %llu", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(SRF02_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"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} /* namespace */ + +int +srf02_i2c_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + srf02_i2c::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + srf02_i2c::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + srf02_i2c::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + srf02_i2c::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + srf02_i2c::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 2b34f360d4..b0dfa194c7 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -314,7 +314,7 @@ ADC::_tick() void ADC::update_system_power(void) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) system_power_s system_power = {}; system_power.timestamp = hrt_absolute_time(); @@ -331,6 +331,15 @@ ADC::update_system_power(void) // publish these to the same topic system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); +#if defined (CONFIG_ARCH_BOARD_MINDPX_V2) + // note that the valid pins are active low + system_power.brick_valid = 1; + system_power.servo_valid = 1; + + // OC pins are active low + system_power.periph_5V_OC = 1; + system_power.hipower_5V_OC = 1; +#else // note that the valid pins are active low system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); @@ -338,6 +347,7 @@ ADC::update_system_power(void) // OC pins are active low system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); +#endif /* lazily publish */ if (_to_system_power != nullptr) { diff --git a/src/drivers/test_ppm/CMakeLists.txt b/src/drivers/test_ppm/CMakeLists.txt new file mode 100644 index 0000000000..666d458cd6 --- /dev/null +++ b/src/drivers/test_ppm/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__test_ppm + MAIN test_ppm + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + test_ppm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/test_ppm/test_ppm.cpp b/src/drivers/test_ppm/test_ppm.cpp new file mode 100644 index 0000000000..5dce26cfe5 --- /dev/null +++ b/src/drivers/test_ppm/test_ppm.cpp @@ -0,0 +1,290 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_ppm.cpp + * + * @author sudragon + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#define TEST_PPM_PIN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + +class TEST_PPM +{ +public: + TEST_PPM(unsigned channels); + virtual ~TEST_PPM(void); + virtual int init(); + unsigned _values[20]; + unsigned _gaps[20]; + unsigned _channels; + unsigned _plus_width; +protected: + +private: + struct hrt_call _call; + unsigned _call_times; + void start(); + + void stop(); + void do_out(); + static void loops(void *arg); + + +}; + + +/** driver 'main' command */ +extern "C" { __EXPORT int test_ppm_main(int argc, char *argv[]); } + +TEST_PPM::TEST_PPM(unsigned channels) : + _channels(channels), + _plus_width(400), + _call{}, + _call_times(0) +{ + memset(&_call, 0, sizeof(_call)); + + for (int i = 0; i < 20; i++) { + _values[i] = 1500; + } + + _values[0] = 5000; +} + +TEST_PPM::~TEST_PPM() +{ + /* make sure we are truly inactive */ + stop(); +} + +int +TEST_PPM::init() +{ + stm32_configgpio(TEST_PPM_PIN); + start(); + return OK; +} + + +void +TEST_PPM::start() +{ + stop(); + _call_times = 0; + hrt_call_after(&_call, 1000, (hrt_callout)&TEST_PPM::loops, this); +} + +void +TEST_PPM::stop() +{ + hrt_cancel(&_call); +} + +void +TEST_PPM::loops(void *arg) +{ + TEST_PPM *dev = reinterpret_cast(arg); + dev->do_out(); +} +void +TEST_PPM::do_out(void) +{ + if ((_call_times % 2) == 0) { + stm32_gpiowrite(TEST_PPM_PIN, false); + hrt_call_after(&_call, _values[_call_times / 2] - _plus_width, (hrt_callout)&TEST_PPM::loops, this); + + } else { + stm32_gpiowrite(TEST_PPM_PIN, true); + hrt_call_after(&_call, _plus_width, (hrt_callout)&TEST_PPM::loops, this); + } + + if ((_call_times / 2) < _channels + 1) { _call_times++; } + + else { _call_times = 0; } +} + +namespace test_ppm +{ + +TEST_PPM *g_test = nullptr; + +void start(unsigned channels); +void stop(); +void usage(); +void set(unsigned ch, unsigned value); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(unsigned channels) +{ + + if (g_test != nullptr) + /* if already started, the still command succeeded */ + { + errx(1, "already started"); + } + + g_test = new TEST_PPM(channels); + + if (g_test == nullptr) { + goto fail; + } + + if (OK != g_test->init()) { + goto fail; + } + + exit(0); +fail: + + if (g_test != nullptr) { + delete(g_test); + g_test = nullptr; + } + + errx(1, "test_ppm start failed"); +} + +void +stop() +{ + if (g_test != nullptr) { + delete g_test; + g_test = nullptr; + + } else { + /* warn, but not an error */ + warnx("test_ppm already stopped."); + } + + exit(0); +} + +void +set(unsigned ch, unsigned value) +{ + if (ch > 18 || ch < 1) {warnx("channel is not valid.");} + + if (value > 2500 || value < 1) { warnx("value is not valid.");} + + g_test->_values[ch] = value; + g_test->_gaps[ch] = 2500 - value; + + if (ch == g_test->_channels) { g_test->_gaps[ch] = 5000; } + + return; +} + +void +usage() +{ + warnx("missing command: try 'start', 'stop', 'set'\n"); +} + +} // namespace + +int +test_ppm_main(int argc, char *argv[]) +{ + const char *verb = argv[1]; + unsigned channels = 7; + + /* + * Start/load the driver. + + */ + + if (!strcmp(verb, "start")) { + test_ppm::start(channels); + exit(0); + } + + if (!strcmp(verb, "stop")) { + + test_ppm::stop(); + exit(0); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "set")) { + if (argc < 4) { + errx(1, "Usage: test_ppm set "); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned value = strtol(argv[3], NULL, 0); + + test_ppm::set(channel, value); + exit(0); + } + + + + test_ppm::usage(); + exit(1); +} diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9c1caf8e83..7084f01467 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,6 +59,10 @@ #define HW_ARCH "AEROCORE" #endif +#ifdef CONFIG_ARCH_BOARD_MINDPX_V2 +#define HW_ARCH "MINDPX_V2" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY #define HW_ARCH "PX4_STM32F4DISCOVERY" #endif diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 999e98c151..65a70401f9 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -36,7 +36,7 @@ * * Definition of esc calibration * - * @author Roman Bapst + * @author Roman Bapst */ #include "esc_calibration.h" @@ -65,6 +65,19 @@ #endif static const int ERROR = -1; +int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) { + struct battery_status_s battery; + memset(&battery,0,sizeof(battery)); + int batt_sub = orb_subscribe(ORB_ID(battery_status)); + orb_copy(ORB_ID(battery_status), batt_sub, &battery); + + if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) { + mavlink_log_info(mavlink_log_pub, "Please disconnect battery and try again!"); + return ERROR; + } + return OK; +} + int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed) { int return_code = OK; diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h index bbf66cb1b3..dd581ba2b4 100644 --- a/src/modules/commander/esc_calibration.h +++ b/src/modules/commander/esc_calibration.h @@ -36,7 +36,7 @@ * * Definition of esc calibration * - * @author Roman Bapst + * @author Roman Bapst */ #ifndef ESC_CALIBRATION_H_ @@ -44,6 +44,7 @@ #include +int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub); int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed); #endif diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7c3b0aadc1..ad76a830f5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -71,6 +71,14 @@ using namespace DriverFramework; #endif +#ifdef CONFIG_ARCH_BOARD_MINDPX_V2 +#define AVIONICS_ERROR_VOLTAGE 3.75f +#define AVIONICS_WARN_VOLTAGE 3.9f +#else +#define AVIONICS_ERROR_VOLTAGE 4.5f +#define AVIONICS_WARN_VOLTAGE 4.9f +#endif + // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -213,11 +221,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.5f) { + if (status->avionics_power_rail_voltage < AVIONICS_ERROR_VOLTAGE) { mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; - } else if (status->avionics_power_rail_voltage < 4.9f) { + } else if (status->avionics_power_rail_voltage < AVIONICS_WARN_VOLTAGE) { mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 531a921b83..43ff2e5b87 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,8 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +112,8 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +156,8 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \ + || defined(CONFIG_ARCH_BOARD_MINDPX_V2) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ba29c4211d..eff6ae80e2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -893,7 +893,7 @@ Sensors::parameters_update() /* apply scaling according to defaults if set to default */ #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) _parameters.battery_voltage_scaling = 0.011f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) _parameters.battery_voltage_scaling = 0.0082f; #elif defined (CONFIG_ARCH_BOARD_AEROCORE) _parameters.battery_voltage_scaling = 0.0063f; @@ -914,7 +914,7 @@ Sensors::parameters_update() #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) /* current scaling for ACSP4 */ _parameters.battery_current_scaling = 0.029296875f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) /* current scaling for 3DR power brick */ _parameters.battery_current_scaling = 0.0124f; #elif defined (CONFIG_ARCH_BOARD_AEROCORE)