From c1812af45ef3815d91d3f0e046cab5dfec501018 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 7 Oct 2016 14:36:49 -1000 Subject: [PATCH] Inital commit of NXPhlite-v1 --- Images/nxphlite-v1.prototype | 12 + cmake/configs/nuttx_nxphlite-v1_default.cmake | 218 ++++ nuttx-configs/nxphlite-v1/Kconfig | 22 + nuttx-configs/nxphlite-v1/include/board.h | 344 +++++ .../nxphlite-v1/include/nsh_romfsimg.h | 42 + nuttx-configs/nxphlite-v1/nsh/Make.defs | 163 +++ nuttx-configs/nxphlite-v1/nsh/defconfig | 1160 +++++++++++++++++ nuttx-configs/nxphlite-v1/nsh/setenv.sh | 75 ++ nuttx-configs/nxphlite-v1/scripts/ld.script | 159 +++ nuttx-configs/nxphlite-v1/src/Makefile | 86 ++ nuttx-configs/nxphlite-v1/src/empty.c | 4 + nuttx-configs/tap-v1/nsh/defconfig | 3 +- .../boards/common/kinetis/board_reset.c | 73 ++ src/drivers/boards/nxphlite-v1/CMakeLists.txt | 55 + src/drivers/boards/nxphlite-v1/board_config.h | 565 ++++++++ .../boards/nxphlite-v1/nxphlite_autoleds.c | 164 +++ .../boards/nxphlite-v1/nxphlite_automount.c | 314 +++++ .../boards/nxphlite-v1/nxphlite_bringup.c | 123 ++ src/drivers/boards/nxphlite-v1/nxphlite_can.c | 129 ++ .../boards/nxphlite-v1/nxphlite_init.c | 479 +++++++ src/drivers/boards/nxphlite-v1/nxphlite_led.c | 107 ++ src/drivers/boards/nxphlite-v1/nxphlite_pwm.c | 106 ++ .../boards/nxphlite-v1/nxphlite_sdhc.c | 249 ++++ .../boards/nxphlite-v1/nxphlite_sdio.c | 120 ++ src/drivers/boards/nxphlite-v1/nxphlite_spi.c | 286 ++++ .../nxphlite-v1/nxphlite_timer_config.c | 60 + src/drivers/boards/nxphlite-v1/nxphlite_usb.c | 118 ++ src/drivers/kinetis/CMakeLists.txt | 45 + src/drivers/kinetis/adc/CMakeLists.txt | 42 + src/drivers/kinetis/adc/adc.cpp | 492 +++++++ src/drivers/kinetis/drv_hrt.c | 911 +++++++++++++ src/drivers/kinetis/drv_input_capture.c | 517 ++++++++ src/drivers/kinetis/drv_input_capture.h | 42 + src/drivers/kinetis/drv_io_timer.c | 763 +++++++++++ src/drivers/kinetis/drv_io_timer.h | 121 ++ src/drivers/kinetis/drv_led_pwm.cpp | 318 +++++ src/drivers/kinetis/drv_pwm_servo.c | 153 +++ src/drivers/kinetis/drv_pwm_servo.h | 42 + src/drivers/kinetis/tone_alarm/CMakeLists.txt | 42 + src/drivers/kinetis/tone_alarm/tone_alarm.cpp | 1019 +++++++++++++++ 40 files changed, 9742 insertions(+), 1 deletion(-) create mode 100644 Images/nxphlite-v1.prototype create mode 100644 cmake/configs/nuttx_nxphlite-v1_default.cmake create mode 100644 nuttx-configs/nxphlite-v1/Kconfig create mode 100644 nuttx-configs/nxphlite-v1/include/board.h create mode 100644 nuttx-configs/nxphlite-v1/include/nsh_romfsimg.h create mode 100644 nuttx-configs/nxphlite-v1/nsh/Make.defs create mode 100644 nuttx-configs/nxphlite-v1/nsh/defconfig create mode 100755 nuttx-configs/nxphlite-v1/nsh/setenv.sh create mode 100644 nuttx-configs/nxphlite-v1/scripts/ld.script create mode 100644 nuttx-configs/nxphlite-v1/src/Makefile create mode 100644 nuttx-configs/nxphlite-v1/src/empty.c create mode 100644 src/drivers/boards/common/kinetis/board_reset.c create mode 100644 src/drivers/boards/nxphlite-v1/CMakeLists.txt create mode 100644 src/drivers/boards/nxphlite-v1/board_config.h create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_autoleds.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_automount.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_bringup.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_can.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_init.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_led.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_pwm.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_sdhc.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_sdio.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_spi.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_timer_config.c create mode 100644 src/drivers/boards/nxphlite-v1/nxphlite_usb.c create mode 100644 src/drivers/kinetis/CMakeLists.txt create mode 100644 src/drivers/kinetis/adc/CMakeLists.txt create mode 100644 src/drivers/kinetis/adc/adc.cpp create mode 100644 src/drivers/kinetis/drv_hrt.c create mode 100644 src/drivers/kinetis/drv_input_capture.c create mode 100644 src/drivers/kinetis/drv_input_capture.h create mode 100644 src/drivers/kinetis/drv_io_timer.c create mode 100644 src/drivers/kinetis/drv_io_timer.h create mode 100644 src/drivers/kinetis/drv_led_pwm.cpp create mode 100644 src/drivers/kinetis/drv_pwm_servo.c create mode 100644 src/drivers/kinetis/drv_pwm_servo.h create mode 100644 src/drivers/kinetis/tone_alarm/CMakeLists.txt create mode 100644 src/drivers/kinetis/tone_alarm/tone_alarm.cpp diff --git a/Images/nxphlite-v1.prototype b/Images/nxphlite-v1.prototype new file mode 100644 index 0000000000..36eb556240 --- /dev/null +++ b/Images/nxphlite-v1.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 28, + "magic": "PX4FWv1", + "description": "Firmware for the NXPHLITEv1 board", + "image": "", + "build_time": 0, + "summary": "NXPHLITEv1", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/cmake/configs/nuttx_nxphlite-v1_default.cmake b/cmake/configs/nuttx_nxphlite-v1_default.cmake new file mode 100644 index 0000000000..f562533540 --- /dev/null +++ b/cmake/configs/nuttx_nxphlite-v1_default.cmake @@ -0,0 +1,218 @@ +include(nuttx/px4_impl_nuttx) + +px4_nuttx_configure(HWCLASS m4 CONFIG nsh ROMFS y ROMFSROOT px4fmu_common) + +set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_uavcan_num_ifaces 1) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/kinetis + drivers/kinetis/adc + drivers/kinetis/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/nxphlite-v1 + drivers/rgbled + drivers/mpu6000 + drivers/mpu9250 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/sf1xx + 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/vmount +# NOT Portable YET drivers/pwm_input + drivers/camera_trigger + drivers/bst + drivers/snapdragon_rc_pwm + drivers/lis3mdl + drivers/bmp280 +#No External SPI drivers/bma180 +#No External SPI drivers/bmi160 + drivers/tap_esc + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib +## Needs bbsrm systemcmds/hardfault_log + systemcmds/reboot + systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + systemcmds/sd_bench + systemcmds/motor_ramp + + # + # Testing + # + drivers/sf0x/sf0x_tests +### NOT Portable YET drivers/test_ppm + modules/commander/commander_tests + modules/controllib_test + modules/mavlink/mavlink_tests + modules/unit_test + modules/uORB/uORB_tests + systemcmds/tests + + # + # General system control + # + modules/commander + modules/load_mon + modules/navigator + modules/mavlink + modules/gpio_led +## modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/attitude_estimator_q + modules/position_estimator_inav + modules/ekf2 + modules/local_position_estimator + + # + # 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 + modules/logger + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/dataman + + # + # Libraries + # + lib/controllib + lib/mathlib + lib/mathlib/math/filter + lib/rc + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + lib/DriverFramework/framework + 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 + + # EKF + examples/ekf_att_pos_estimator +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_extra_libs +# uavcan +# uavcan_stm32_driver + ) + +set(config_io_extra_libs + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "sercon" + STACK_MAIN "2048" + COMPILE_FLAGS "-Os") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + PRIORITY "SCHED_PRIORITY_DEFAULT" + MAIN "serdis" + STACK_MAIN "2048" + COMPILE_FLAGS "-Os") diff --git a/nuttx-configs/nxphlite-v1/Kconfig b/nuttx-configs/nxphlite-v1/Kconfig new file mode 100644 index 0000000000..9a67619f3b --- /dev/null +++ b/nuttx-configs/nxphlite-v1/Kconfig @@ -0,0 +1,22 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_NXPHLITE_V1 + +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-6 to provide timing signals from selected drivers. + +endif diff --git a/nuttx-configs/nxphlite-v1/include/board.h b/nuttx-configs/nxphlite-v1/include/board.h new file mode 100644 index 0000000000..6801d9dc0d --- /dev/null +++ b/nuttx-configs/nxphlite-v1/include/board.h @@ -0,0 +1,344 @@ +/************************************************************************************ + * configs/freedom-k64f/include/board.h + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * Jordan MacIntyre + * David Sidrane + * + * + * 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 __CONFIG_NXPHLITE_V1_INCLUDE_BOARD_H +#define __CONFIG_NXPHLITE_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* + * The NXPHlite-v1 is populated with a MK64FX512VLQ12 has 512 KiB of FLASH and + * 192KiB of SRAM. + */ +/* Clocking *************************************************************************/ +/* The NXPHlite-v1 uses a 16MHz external Oscillator. The Kinetis MCU startup from an + * internal digitally-controlled oscillator (DCO). Nuttx will enable the main external + * oscillator (EXTAL0/XTAL0). The external oscillator/resonator can range from + * 32.768 KHz up to 50 MHz. The default external source for the MCG oscillator inputs + * EXTAL. + * + * Y1 a High-frequency, low-power Xtal + */ +#define BOARD_EXTAL_LP 1 +#define BOARD_EXTAL_FREQ 16000000 /* 16MHz Oscillator Y1 */ +#define BOARD_XTAL32_FREQ 32768 /* 32KHz RTC Oscillator */ + +/* PLL Configuration. Either the external clock or crystal frequency is used to + * select the PRDIV value. Only reference clock frequencies are supported that will + * produce a 2-4 MHz reference clock to the PLL. + * + * PLL Input frequency: PLLIN = REFCLK / PRDIV = 16 MHz / 4 = 4 MHz + * PLL Output frequency: PLLOUT = PLLIN * VDIV = 4 MHz * 30 = 120 MHz + * MCG Frequency: PLLOUT = 120 MHz + * + * PRDIV register value is the divider minus one. So 4 -> 3 + * VDIV register value is offset by 24. So 30 -> 6 + */ + +#define BOARD_PRDIV 4 /* PLL External Reference Divider */ +#define BOARD_VDIV 30 /* PLL VCO Divider (frequency multiplier) */ + +#define BOARD_PLLIN_FREQ (BOARD_EXTAL_FREQ / BOARD_PRDIV) +#define BOARD_PLLOUT_FREQ (BOARD_PLLIN_FREQ * BOARD_VDIV) +#define BOARD_MCG_FREQ BOARD_PLLOUT_FREQ + +/* SIM CLKDIV1 dividers */ + +#define BOARD_OUTDIV1 1 /* Core = MCG, 120 MHz */ +#define BOARD_OUTDIV2 2 /* Bus = MCG / 2, 60 MHz */ +#define BOARD_OUTDIV3 2 /* FlexBus = MCG / 2, 60 MHz */ +#define BOARD_OUTDIV4 5 /* Flash clock = MCG / 5, 24 MHz */ + +#define BOARD_CORECLK_FREQ (BOARD_MCG_FREQ / BOARD_OUTDIV1) +#define BOARD_BUS_FREQ (BOARD_MCG_FREQ / BOARD_OUTDIV2) +#define BOARD_FLEXBUS_FREQ (BOARD_MCG_FREQ / BOARD_OUTDIV3) +#define BOARD_FLASHCLK_FREQ (BOARD_MCG_FREQ / BOARD_OUTDIV4) + +/* SDHC clocking ********************************************************************/ + +/* SDCLK configurations corresponding to various modes of operation. Formula is: + * + * SDCLK frequency = (base clock) / (prescaler * divisor) + * + * The SDHC module is always configure configured so that the core clock is the base + * clock. Possible values for presscaler and divisor are: + * + * SDCLKFS: {2, 4, 8, 16, 32, 63, 128, 256} + * DVS: {1..16} + */ + +/* Identification mode: Optimal 400KHz, Actual 120MHz / (32 * 10) = 375 KHz */ + +#define BOARD_SDHC_IDMODE_PRESCALER SDHC_SYSCTL_SDCLKFS_DIV32 +#define BOARD_SDHC_IDMODE_DIVISOR SDHC_SYSCTL_DVS_DIV(10) + +/* MMC normal mode: Optimal 20MHz, Actual 120MHz / (2 * 3) = 20 MHz */ + +#define BOARD_SDHC_MMCMODE_PRESCALER SDHC_SYSCTL_SDCLKFS_DIV2 +#define BOARD_SDHC_MMCMODE_DIVISOR SDHC_SYSCTL_DVS_DIV(3) + +/* SD normal mode (1-bit): Optimal 20MHz, Actual 120MHz / (2 * 3) = 20 MHz */ + +#define BOARD_SDHC_SD1MODE_PRESCALER SDHC_SYSCTL_SDCLKFS_DIV2 +#define BOARD_SDHC_SD1MODE_DIVISOR SDHC_SYSCTL_DVS_DIV(3) + +/* SD normal mode (4-bit): Optimal 25MHz, Actual 120MHz / (2 * 3) = 20 MHz (with DMA) + * SD normal mode (4-bit): Optimal 20MHz, Actual 120MHz / (2 * 3) = 20 MHz (no DMA) + */ + +#ifdef CONFIG_SDIO_DMA +# define BOARD_SDHC_SD4MODE_PRESCALER SDHC_SYSCTL_SDCLKFS_DIV2 +# define BOARD_SDHC_SD4MODE_DIVISOR SDHC_SYSCTL_DVS_DIV(3) +#else +# define BOARD_SDHC_SD4MODE_PRESCALER SDHC_SYSCTL_SDCLKFS_DIV2 +# define BOARD_SDHC_SD4MODE_DIVISOR SDHC_SYSCTL_DVS_DIV(3) +#endif + +/* LED definitions ******************************************************************/ +/* The NXPHlite-v1 has a separate Red, Green and Blue LEDs driven by the K64F as + * follows: + * + * LED K64 + * ------ ------------------------------------------------------- + * RED FB_CS3_b/ FB_BE7_0_BLS31_24_b/ FB_TA_b/ ENET0_1588_TMR3/ UART3_CTS_b/ PTC19 + * GREEN LLWU_P12/ FB_CS1_b/ FB_TS_b/ FB_ALE/ FTM3_CH0/ UART2_RTS_b/ SPI0_PCS0/ PTD0 + * BLUE FB_CS0_b/ FTM3_CH1/ UART2_CTS_b/ SPI0_SCK/ ADC0_SE5b/ PTD1 + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED_R 0 +#define BOARD_LED_G 1 +#define BOARD_LED_B 2 +#define BOARD_NLEDS 3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED_R_BIT (1 << BOARD_LED_R) +#define BOARD_LED_G_BIT (1 << BOARD_LED_G) +#define BOARD_LED_B_BIT (1 << BOARD_LED_B) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the NXPHlite-v1. The following definitions describe how NuttX controls + * the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ------------------- ---------------------------- ----------------- */ +#define LED_STARTED 1 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 2 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 0 /* Interrupts enabled OFF OFF ON */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON OFF */ +#define LED_INIRQ 0 /* In an interrupt (no change) */ +#define LED_SIGNAL 0 /* In a signal handler (no change) */ +#define LED_ASSERTION 0 /* An assertion failed (no change) */ +#define LED_PANIC 4 /* The system has crashed FLASH OFF OFF */ +#undef LED_IDLE /* K64 is in sleep mode (Not used) */ + +/* Alternative pin resolution *******************************************************/ +/* If there are alternative configurations for various pins in the + * kinetis_k64pinmux.h header file, those alternative pins will be labeled with a + * suffix like _1, _2, etc. The logic in this file must select the correct pin + * configuration for the board by defining a pin configuration (with no suffix) that + * maps to the correct alternative. + */ + +/* CAN + * + */ +#define PIN_CAN0_RX PIN_CAN0_RX_2 +#define PIN_CAN0_TX PIN_CAN0_TX_2 + +/* 12C + * + */ + +/* I2C0 MPL3115A2 Pressure Sensor */ + +#define PIN_I2C0_SCL PIN_I2C0_SCL_4 /* PTE24 P_SCL */ +#define PIN_I2C0_SDA PIN_I2C0_SDA_4 /* PTE25 P_SDA */ + +/* I2C1 NFC Connector */ + +#define PIN_I2C1_SCL PIN_I2C1_SCL_1 /* PTC10 NFC_SCL P2-2 */ +#define PIN_I2C1_SDA PIN_I2C1_SDA_1 /* PTC11 NFC_SDA P2-3 */ + + +/* PWM + * + */ + +/* PWM Channels */ + +#define GPIO_FTM3_CH0OUT PIN_FTM3_CH0_2 /* PTE5 PWM1 P4-34 */ +#define GPIO_FTM3_CH1OUT PIN_FTM3_CH1_2 /* PTE6 PWM2 P4-31 */ +#define GPIO_FTM3_CH2OUT PIN_FTM3_CH2_2 /* PTE7 PWM3 P4-28 */ +#define GPIO_FTM3_CH3OUT PIN_FTM3_CH3_2 /* PTE8 PWM4 P4-25 */ +#define GPIO_FTM3_CH4OUT PIN_FTM3_CH4_2 /* PTE9 PWM5 P4-22 */ +#define GPIO_FTM3_CH5OUT PIN_FTM3_CH5_2 /* PTE10 PWM6 P4-29 */ +#define GPIO_FTM3_CH6OUT PIN_FTM3_CH6_2 /* PTE11 PWM7 P4-26 */ +#define GPIO_FTM3_CH7OUT PIN_FTM3_CH7_2 /* PTE12 PWM8 P4-13 */ + +#define GPIO_FTM0_CH4OUT PIN_FTM0_CH4_3 /* PTD4 PWM0 P4-46 */ +#define GPIO_FTM0_CH5OUT PIN_FTM0_CH5_3 /* PTD5 PWM10 P4-43 */ +#define GPIO_FTM0_CH6OUT PIN_FTM0_CH6_2 /* PTD6 PWM11 P4-40 */ +#define GPIO_FTM0_CH7OUT PIN_FTM0_CH7_2 /* PTD7 PWM12 P4-37 */ + +#define GPIO_FTM0_CH3OUT PIN_FTM0_CH3_1 /* PTA6 PWM13 P4-7 */ +#define GPIO_FTM0_CH2OUT PIN_FTM0_CH2_2 /* PTC3 PWM14 P4-10 */ + +//todo:This is a Guess on timer utilisation +#define GPIO_FTM3_CH0IN PIN_FTM3_CH0_2 /* PTE5 PWM1 P4-34 */ +#define GPIO_FTM3_CH1IN PIN_FTM3_CH1_2 /* PTE6 PWM2 P4-31 */ +#define GPIO_FTM3_CH2IN PIN_FTM3_CH2_2 /* PTE7 PWM3 P4-28 */ +#define GPIO_FTM3_CH3IN PIN_FTM3_CH3_2 /* PTE8 PWM4 P4-25 */ +#define GPIO_FTM3_CH4IN PIN_FTM3_CH4_2 /* PTE9 PWM5 P4-22 */ +#define GPIO_FTM3_CH5IN PIN_FTM3_CH5_2 /* PTE10 PWM6 P4-29 */ +#define GPIO_FTM3_CH6IN PIN_FTM3_CH6_2 /* PTE11 PWM7 P4-26 */ +#define GPIO_FTM3_CH7IN PIN_FTM3_CH7_2 /* PTE12 PWM8 P4-13 */ + +#define GPIO_FTM0_CH4IN PIN_FTM0_CH4_3 /* PTD4 PWM0 P4-46 */ +#define GPIO_FTM0_CH5IN PIN_FTM0_CH5_3 /* PTD5 PWM10 P4-43 */ +#define GPIO_FTM0_CH6IN PIN_FTM0_CH6_2 /* PTD6 PWM11 P4-40 */ +#define GPIO_FTM0_CH7IN PIN_FTM0_CH7_2 /* PTD7 PWM12 P4-37 */ + +#define GPIO_FTM0_CH3IN PIN_FTM0_CH3_1 /* PTA6 PWM13 P4-7 */ +#define GPIO_FTM0_CH2IN PIN_FTM0_CH2_2 /* PTC3 PWM14 P4-10 */ + +/* SPI + * + */ + +/* SPI0 SD Card */ + +#define PIN_SPI0_PCS0 PIN_SPI0_PCS0_2 /* PTC4 SPI_CS SD1-2 */ +#define PIN_SPI0_SCK PIN_SPI0_SCK_2 /* PTC5 SPI_CLK SD1-5 */ +#define PIN_SPI0_OUT PIN_SPI0_SOUT_2 /* PTC6 SPI_OUT SD1-3 */ +#define PIN_SPI0_SIN PIN_SPI0_SIN_2 /* PTC7 SPI_IN SD1-5 */ + +/* SPI1 FXOS8700CQ Accelerometer */ + +#define PIN_SPI1_PCS0 PIN_SPI1_PCS0_1 /* PTB10 A_CS */ +#define PIN_SPI1_SCK PIN_SPI1_SCK_1 /* PTB11 A_SCLK */ +#define PIN_SPI1_OUT PIN_SPI1_SOUT_1 /* PTB16 A_MOSI */ +#define PIN_SPI1_SIN PIN_SPI1_SIN_1 /* PTB17 A_MISO */ + +/* SPI2 FXAS21002CQ Gyroscope */ + +#define PIN_SPI2_PCS0 PIN_SPI2_PCS0_1 /* PTB20 GM_CS */ +#define PIN_SPI2_SCK PIN_SPI2_SCK_1 /* PTB21 GM_SCLK */ +#define PIN_SPI2_OUT PIN_SPI2_SOUT_1 /* PTB22 GM_MOSI */ +#define PIN_SPI2_SIN PIN_SPI2_SIN_1 /* PTB23 GM_MISO */ + +/* UART + * + * NuttX Will use UART4 as the Console + */ + +#define PIN_UART0_RX PIN_UART0_RX_4 /* PTD6 P4-40 PWM11 */ +#define PIN_UART0_TX PIN_UART0_TX_4 /* PTD7 P4-37 PWM12 */ + +#define PIN_UART1_RX PIN_UART1_RX_2 /* PTE1 UART P14-3 */ +#define PIN_UART1_TX PIN_UART1_TX_2 /* PTE0 UART P14-2 */ + +/* No Alternative pins for UART2 + * PD2 BL P1-5 + * PD3 BL P1-4 + */ + +#define PIN_UART3_RX PIN_UART3_RX_2 /* PTC16 GPS P3-3 */ +#define PIN_UART3_TX PIN_UART3_TX_2 /* PTC17 GPS P3-2 */ + +#define PIN_UART4_RX PIN_UART4_RX_1 /* PTC14 UART P10-3 */ +#define PIN_UART4_TX PIN_UART4_TX_1 /* PTC15 UART P10-2 */ + +/* UART5 is not connected on V1 + */ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +/************************************************************************************ + * Name: kinetis_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. + * + ************************************************************************************/ + +void kinetis_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_NXPHLITE_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/nxphlite-v1/include/nsh_romfsimg.h b/nuttx-configs/nxphlite-v1/include/nsh_romfsimg.h new file mode 100644 index 0000000000..beb1a924cf --- /dev/null +++ b/nuttx-configs/nxphlite-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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/nxphlite-v1/nsh/Make.defs b/nuttx-configs/nxphlite-v1/nsh/Make.defs new file mode 100644 index 0000000000..4b5898426b --- /dev/null +++ b/nuttx-configs/nxphlite-v1/nsh/Make.defs @@ -0,0 +1,163 @@ +############################################################################ +# nuttx-configs/nxphlite-v1/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 +include $(TOPDIR)/PX4_Warnings.mk +include $(TOPDIR)/PX4_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${HOST_OS_FIRST_LETTER} + +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$(HOSTEXEEXT) + 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} + +# Optimization 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 = $(PX4_ARCHWARNINGS) +ARCHCWARNINGS = $(PX4_ARCHWARNINGS) $(PX4_ARCHCWARNINGS) +ARCHWARNINGSXX = $(ARCHWARNINGS) $(PX4_ARCHWARNINGSXX) +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/nxphlite-v1/nsh/defconfig b/nuttx-configs/nxphlite-v1/nsh/defconfig new file mode 100644 index 0000000000..48783a2afe --- /dev/null +++ b/nuttx-configs/nxphlite-v1/nsh/defconfig @@ -0,0 +1,1160 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_DEFAULT_SMALL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +CONFIG_BUILD_FLAT=y +# 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 +# CONFIG_UBOOT_UIMAGE is not set + +# +# Customize Header Files +# +# CONFIG_ARCH_STDINT_H is not set +# 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 +# CONFIG_ARCH_DEBUG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG_ALERT=y +# CONFIG_DEBUG_FEATURES is not set +CONFIG_ARCH_HAVE_STACKCHECK=y +CONFIG_STACK_COLORATION=y +# CONFIG_ARCH_HAVE_HEAPCHECK is not set +CONFIG_DEBUG_SYMBOLS=y +CONFIG_ARCH_HAVE_CUSTOMOPT=y +CONFIG_DEBUG_NOOPT=y +# CONFIG_DEBUG_CUSTOMOPT is not set +# CONFIG_DEBUG_FULLOPT is not set + +# +# System Type +# +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_RENESAS 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_A1X is not set +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_EFM32 is not set +# CONFIG_ARCH_CHIP_IMX1 is not set +# CONFIG_ARCH_CHIP_IMX6 is not set +CONFIG_ARCH_CHIP_KINETIS=y +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_TIVA is not set +# CONFIG_ARCH_CHIP_LPC11XX 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_SAMA5 is not set +# CONFIG_ARCH_CHIP_SAMD is not set +# CONFIG_ARCH_CHIP_SAML is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +# CONFIG_ARCH_CHIP_SAMV7 is not set +# CONFIG_ARCH_CHIP_STM32 is not set +# CONFIG_ARCH_CHIP_STM32F7 is not set +# CONFIG_ARCH_CHIP_STM32L4 is not set +# CONFIG_ARCH_CHIP_STR71X is not set +# CONFIG_ARCH_CHIP_TMS570 is not set +# CONFIG_ARCH_CHIP_MOXART is not set +# CONFIG_ARCH_ARM7TDMI is not set +# CONFIG_ARCH_ARM926EJS is not set +# CONFIG_ARCH_ARM920T is not set +# CONFIG_ARCH_CORTEXM0 is not set +# CONFIG_ARCH_CORTEXM3 is not set +CONFIG_ARCH_CORTEXM4=y +# CONFIG_ARCH_CORTEXM7 is not set +# CONFIG_ARCH_CORTEXA5 is not set +# CONFIG_ARCH_CORTEXA8 is not set +# CONFIG_ARCH_CORTEXA9 is not set +# CONFIG_ARCH_CORTEXR4 is not set +# CONFIG_ARCH_CORTEXR4F is not set +# CONFIG_ARCH_CORTEXR5 is not set +# CONFIG_ARCH_CORTEX5F is not set +# CONFIG_ARCH_CORTEXR7 is not set +# CONFIG_ARCH_CORTEXR7F is not set +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="kinetis" +# CONFIG_ARM_TOOLCHAIN_IAR is not set +CONFIG_ARM_TOOLCHAIN_GNU=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARMV7M_LAZYFPU is not set +CONFIG_ARCH_HAVE_FPU=y +# CONFIG_ARCH_HAVE_DPFPU is not set +CONFIG_ARCH_FPU=y +# CONFIG_ARCH_HAVE_TRUSTZONE is not set +CONFIG_ARM_HAVE_MPU_UNIFIED=y +# CONFIG_ARM_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_HAVE_ICACHE is not set +# CONFIG_ARMV7M_HAVE_DCACHE is not set +# CONFIG_ARMV7M_HAVE_ITCM is not set +# CONFIG_ARMV7M_HAVE_DTCM is not set +# CONFIG_ARMV7M_TOOLCHAIN_IARL is not set +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y +# CONFIG_ARMV7M_HAVE_STACKCHECK is not set +# CONFIG_ARMV7M_ITMSYSLOG is not set + +# +# Kinetis Configuration Options +# +# CONFIG_ARCH_CHIP_MK20DN32VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DX32VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DN64VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DX64VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DN128VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DX128VLH5 is not set +# CONFIG_ARCH_CHIP_MK20DX64VLH7 is not set +# CONFIG_ARCH_CHIP_MK20DX128VLH7 is not set +# CONFIG_ARCH_CHIP_MK20DX256VLH7 is not set +# CONFIG_ARCH_CHIP_MK40N512VLQ100 is not set +# CONFIG_ARCH_CHIP_MK40N512VMD100 is not set +# CONFIG_ARCH_CHIP_MK40X128VLQ100 is not set +# CONFIG_ARCH_CHIP_MK40X128VMD100 is not set +# CONFIG_ARCH_CHIP_MK40X256VLQ100 is not set +# CONFIG_ARCH_CHIP_MK40X256VMD100 is not set +# CONFIG_ARCH_CHIP_MK60N256VLQ100 is not set +# CONFIG_ARCH_CHIP_MK60N256VMD100 is not set +# CONFIG_ARCH_CHIP_MK60N512VLL100 is not set +# CONFIG_ARCH_CHIP_MK60N512VLQ100 is not set +# CONFIG_ARCH_CHIP_MK60N512VMD100 is not set +# CONFIG_ARCH_CHIP_MK60X256VLQ100 is not set +# CONFIG_ARCH_CHIP_MK60X256VMD100 is not set +# CONFIG_ARCH_CHIP_MK64FN1M0VLL12 is not set +# CONFIG_ARCH_CHIP_MK64FX512VLL12 is not set +# CONFIG_ARCH_CHIP_MK64FX512VDC12 is not set +# CONFIG_ARCH_CHIP_MK64FN1M0VDC12 is not set +CONFIG_ARCH_CHIP_MK64FX512VLQ12=y +# CONFIG_ARCH_CHIP_MK64FX512VMD12 is not set +# CONFIG_ARCH_CHIP_MK64FN1M0VMD12 is not set +# CONFIG_ARCH_FAMILY_K20 is not set +# CONFIG_ARCH_FAMILY_K40 is not set +# CONFIG_ARCH_FAMILY_K60 is not set +CONFIG_ARCH_FAMILY_K64=y + +# +# Kinetis Peripheral Support +# +CONFIG_KINETIS_HAVE_I2C1=y +CONFIG_KINETIS_HAVE_I2C2=y +# CONFIG_KINETIS_TRACE is not set +# CONFIG_KINETIS_FLEXBUS is not set +CONFIG_KINETIS_UART0=y +CONFIG_KINETIS_UART1=y +CONFIG_KINETIS_UART2=y +CONFIG_KINETIS_UART3=y +CONFIG_KINETIS_UART4=y +# CONFIG_KINETIS_UART5 is not set +# CONFIG_KINETIS_ENET is not set +# CONFIG_KINETIS_RNGB is not set +CONFIG_KINETIS_FLEXCAN0=y +# CONFIG_KINETIS_FLEXCAN1 is not set +CONFIG_KINETIS_SPI0=y +CONFIG_KINETIS_SPI1=y +CONFIG_KINETIS_SPI2=y +CONFIG_KINETIS_I2C0=y +CONFIG_KINETIS_I2C1=y +# CONFIG_KINETIS_I2C2 is not set +# CONFIG_KINETIS_I2S is not set +# CONFIG_KINETIS_DAC0 is not set +# CONFIG_KINETIS_DAC1 is not set +CONFIG_KINETIS_ADC0=y +CONFIG_KINETIS_ADC1=y +# CONFIG_KINETIS_CMP is not set +# CONFIG_KINETIS_VREF is not set +# CONFIG_KINETIS_SDHC is not set +CONFIG_KINETIS_FTM0=y +CONFIG_KINETIS_FTM1=y +CONFIG_KINETIS_FTM2=y +# CONFIG_KINETIS_LPTIMER is not set +CONFIG_KINETIS_RTC=y +# CONFIG_KINETIS_EWM is not set +# CONFIG_KINETIS_CMT is not set +CONFIG_KINETIS_USBOTG=y +CONFIG_KINETIS_USBDCD=y +# CONFIG_KINETIS_LLWU is not set +# CONFIG_KINETIS_TSI is not set +# CONFIG_KINETIS_FTFL is not set +CONFIG_KINETIS_DMA=y +# CONFIG_KINETIS_CRC is not set +CONFIG_KINETIS_PDB=y +CONFIG_KINETIS_PIT=y + +# +# Kinetis FTM PWM Configuration +# +# CONFIG_KINETIS_FTM0_PWM is not set +# CONFIG_KINETIS_FTM1_PWM is not set +# CONFIG_KINETIS_FTM2_PWM is not set + +# +# Kinetis GPIO Interrupt Configuration +# +CONFIG_KINETIS_GPIOIRQ=y +CONFIG_KINETIS_PORTAINTS=y +CONFIG_KINETIS_PORTBINTS=y +CONFIG_KINETIS_PORTCINTS=y +CONFIG_KINETIS_PORTDINTS=y +CONFIG_KINETIS_PORTEINTS=y + +# +# Kinetis UART Configuration +# +CONFIG_KINETIS_UARTFIFOS=y + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +# CONFIG_ARCH_DMA is not set +CONFIG_ARCH_HAVE_IRQPRIO=y +# CONFIG_ARCH_L2CACHE is not set +# CONFIG_ARCH_HAVE_COHERENT_DCACHE is not set +# CONFIG_ARCH_HAVE_ADDRENV is not set +# CONFIG_ARCH_NEED_ADDRENV_MAPPING is not set +# CONFIG_ARCH_HAVE_MULTICPU is not set +CONFIG_ARCH_HAVE_VFORK=y +# CONFIG_ARCH_HAVE_MMU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARCH_NAND_HWECC is not set +# CONFIG_ARCH_HAVE_EXTCLK is not set +# CONFIG_ARCH_HAVE_POWEROFF is not set +CONFIG_ARCH_HAVE_RESET=y +# CONFIG_ARCH_USE_MPU is not set +# CONFIG_ARCH_IRQPRIO is not set +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_IDLE_CUSTOM is not set +CONFIG_ARCH_HAVE_RAMFUNCS=y +CONFIG_ARCH_RAMFUNCS=y +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=10016 +# CONFIG_ARCH_CALIBRATION is not set + +# +# Interrupt options +# +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y +# CONFIG_ARCH_HIPRI_INTERRUPT is not set + +# +# 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 + +# +# Boot Memory Configuration +# +CONFIG_RAM_START=0x1FFE0000 +CONFIG_RAM_SIZE=196608 +# CONFIG_ARCH_HAVE_SDRAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_NXPHLITE_V1=y +CONFIG_ARCH_BOARD="nxphlite-v1" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +CONFIG_ARCH_LEDS=y + +# +# Board-Specific Options +# +CONFIG_BOARD_HAS_PROBES=y +CONFIG_BOARD_USE_PROBES=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_LIB_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +# CONFIG_BOARDCTL_UNIQUEID is not set +CONFIG_BOARDCTL_USBDEVCTRL=y +# CONFIG_BOARDCTL_TSCTEST is not set +# CONFIG_BOARDCTL_ADCTEST is not set +# CONFIG_BOARDCTL_PWMTEST is not set +# CONFIG_BOARDCTL_GRAPHICS is not set +# CONFIG_BOARDCTL_IOCTL is not set + +# +# RTOS Features +# +# CONFIG_DISABLE_OS_API is not set + +# +# Clocks and Timers +# +CONFIG_USEC_PER_TICK=1000 +# CONFIG_SYSTEM_TIME64 is not set +# CONFIG_CLOCK_MONOTONIC is not set +# CONFIG_ARCH_HAVE_TIMEKEEPING is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=2015 +CONFIG_START_MONTH=11 +CONFIG_START_DAY=30 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_WDOG_INTRESERVE=4 +CONFIG_PREALLOC_TIMERS=50 + +# +# Tasks and Scheduling +# +# CONFIG_INIT_NONE is not set +CONFIG_INIT_ENTRYPOINT=y +# CONFIG_INIT_FILEPATH is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_RR_INTERVAL=0 +# CONFIG_SCHED_SPORADIC is not set +CONFIG_TASK_NAME_SIZE=24 +CONFIG_MAX_TASKS=32 +# CONFIG_SCHED_HAVE_PARENT is not set +CONFIG_SCHED_WAITPID=y + +# +# Pthread Options +# +# CONFIG_MUTEX_TYPES is not set +CONFIG_NPTHREAD_KEYS=4 + +# +# Performance Monitoring +# +# CONFIG_SCHED_CPULOAD is not set +CONFIG_SCHED_INSTRUMENTATION=y +# CONFIG_SCHED_INSTRUMENTATION_PREEMPTION is not set +# CONFIG_SCHED_INSTRUMENTATION_CSECTION is not set +# CONFIG_SCHED_INSTRUMENTATION_BUFFER is not set + +# +# Files and I/O +# +CONFIG_DEV_CONSOLE=y +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NFILE_DESCRIPTORS=51 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 + +# +# RTOS hooks +# +# CONFIG_BOARD_INITIALIZE is not set +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +# CONFIG_SIG_EVTHREAD 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 + +# +# POSIX Message Queue Options +# +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +# CONFIG_MODULE is not set + +# +# Work queue support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=192 +CONFIG_SCHED_HPWORKPERIOD=5000 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPNTHREADS=1 +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPRIOMAX=176 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=500 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +# CONFIG_LIB_SYSCALL is not set + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_DEV_URANDOM is not set +# CONFIG_DEV_LOOP is not set + +# +# Buffering +# +# CONFIG_DRVR_WRITEBUFFER is not set +# CONFIG_DRVR_READAHEAD is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set +# CONFIG_ARCH_HAVE_PWM_MULTICHAN is not set +# CONFIG_PWM is not set +# CONFIG_ARCH_HAVE_I2CRESET is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +# CONFIG_I2C_POLLED is not set +CONFIG_I2C_RESET=y +# CONFIG_I2C_TRACE is not set +# CONFIG_I2C_DRIVER is not set +CONFIG_SPI=y +# CONFIG_SPI_SLAVE is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_SPI_CALLBACK is not set +# CONFIG_SPI_HWFEATURES is not set +# CONFIG_ARCH_HAVE_SPI_CRCGENERATION is not set +# CONFIG_ARCH_HAVE_SPI_CS_CONTROL is not set +# CONFIG_ARCH_HAVE_SPI_BITORDER is not set +# CONFIG_SPI_CS_DELAY_CONTROL is not set +# CONFIG_SPI_DRIVER is not set +# CONFIG_SPI_BITBANG is not set +# CONFIG_I2S is not set + +# +# Timer Driver Support +# +# CONFIG_TIMER is not set +# CONFIG_ONESHOT is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +# CONFIG_RTC_ALARM is not set +# CONFIG_RTC_DRIVER is not set +# CONFIG_RTC_EXTERNAL is not set +CONFIG_WATCHDOG=y +CONFIG_WATCHDOG_DEVPATH="/dev/watchdog0" +# CONFIG_TIMERS_CS2100CP is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_VIDEO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set + +# +# IO Expander/GPIO Support +# +# CONFIG_IOEXPANDER is not set +# CONFIG_DEV_GPIO is not set + +# +# LCD Driver Support +# +# CONFIG_LCD is not set +# CONFIG_SLCD is not set + +# +# LED Support +# +# CONFIG_USERLED is not set +# CONFIG_RGBLED is not set +# CONFIG_PCA9635PW is not set +# CONFIG_NCP5623C 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=y +CONFIG_MMCSD_SPICLOCK=24000000 +CONFIG_MMCSD_SPIMODE=0 +# CONFIG_ARCH_HAVE_SDIO is not set +# CONFIG_ARCH_HAVE_SDIOWAIT_WRCOMPLETE is not set +# CONFIG_MODEM is not set +# CONFIG_MTD is not set +# CONFIG_EEPROM is not set +CONFIG_PIPES=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=1024 +CONFIG_DEV_FIFO_SIZE=0 +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +CONFIG_SERIAL_CONSOLE=y +# CONFIG_16550_UART is not set +# CONFIG_UART_SERIALDRIVER is not set +CONFIG_UART0_SERIALDRIVER=y +CONFIG_UART1_SERIALDRIVER=y +CONFIG_UART2_SERIALDRIVER=y +CONFIG_UART3_SERIALDRIVER=y +CONFIG_UART4_SERIALDRIVER=y +# CONFIG_UART5_SERIALDRIVER is not set +# CONFIG_UART6_SERIALDRIVER is not set +# CONFIG_UART7_SERIALDRIVER is not set +# CONFIG_UART8_SERIALDRIVER is not set +# CONFIG_SCI0_SERIALDRIVER is not set +# CONFIG_SCI1_SERIALDRIVER is not set +# CONFIG_USART0_SERIALDRIVER is not set +# CONFIG_USART1_SERIALDRIVER is not set +# CONFIG_USART2_SERIALDRIVER is not set +# CONFIG_USART3_SERIALDRIVER is not set +# CONFIG_USART4_SERIALDRIVER is not set +# CONFIG_USART5_SERIALDRIVER is not set +# CONFIG_USART6_SERIALDRIVER is not set +# CONFIG_USART7_SERIALDRIVER is not set +# CONFIG_USART8_SERIALDRIVER is not set +# CONFIG_OTHER_UART_SERIALDRIVER is not set +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +# CONFIG_SERIAL_DMA is not set +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_IFLOWCONTROL_LOWER_WATERMARK=10 +CONFIG_SERIAL_IFLOWCONTROL_UPPER_WATERMARK=90 +# CONFIG_ARCH_HAVE_SERIAL_TERMIOS is not set +# CONFIG_UART0_SERIAL_CONSOLE is not set +# CONFIG_UART1_SERIAL_CONSOLE is not set +# CONFIG_UART2_SERIAL_CONSOLE is not set +# CONFIG_UART3_SERIAL_CONSOLE is not set +CONFIG_UART4_SERIAL_CONSOLE=y +# CONFIG_OTHER_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# UART0 Configuration +# +CONFIG_UART0_RXBUFSIZE=256 +CONFIG_UART0_TXBUFSIZE=256 +CONFIG_UART0_BAUD=115200 +CONFIG_UART0_BITS=8 +CONFIG_UART0_PARITY=0 +CONFIG_UART0_2STOP=0 +# CONFIG_UART0_IFLOWCONTROL is not set +# CONFIG_UART0_OFLOWCONTROL is not set +# CONFIG_UART0_DMA is not set + +# +# UART1 Configuration +# +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1100 +CONFIG_UART1_BAUD=115200 +CONFIG_UART1_BITS=8 +CONFIG_UART1_PARITY=0 +CONFIG_UART1_2STOP=0 +CONFIG_UART1_IFLOWCONTROL=y +CONFIG_UART1_OFLOWCONTROL=y +# CONFIG_UART1_DMA is not set + +# +# UART2 Configuration +# +CONFIG_UART2_RXBUFSIZE=256 +CONFIG_UART2_TXBUFSIZE=256 +CONFIG_UART2_BAUD=115200 +CONFIG_UART2_BITS=8 +CONFIG_UART2_PARITY=0 +CONFIG_UART2_2STOP=0 +# CONFIG_UART2_IFLOWCONTROL is not set +# CONFIG_UART2_OFLOWCONTROL is not set +# CONFIG_UART2_DMA is not set + +# +# UART3 Configuration +# +CONFIG_UART3_RXBUFSIZE=256 +CONFIG_UART3_TXBUFSIZE=256 +CONFIG_UART3_BAUD=115200 +CONFIG_UART3_BITS=8 +CONFIG_UART3_PARITY=0 +CONFIG_UART3_2STOP=0 +# CONFIG_UART3_IFLOWCONTROL is not set +# CONFIG_UART3_OFLOWCONTROL is not set +# CONFIG_UART3_DMA is not set + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 +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 +# CONFIG_UART4_DMA is not set +# CONFIG_PSEUDOTERM is not set +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_ARCH_USBDEV_STALLQUEUE 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_NRDREQS=4 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="NXP" +CONFIG_CDCACM_PRODUCTSTR="PX4 NXPHlite v1.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_HAVE_USBTRACE is not set +# CONFIG_DRIVERS_WIRELESS is not set +# CONFIG_DRIVERS_CONTACTLESS is not set + +# +# System Logging +# +# CONFIG_ARCH_SYSLOG is not set +# CONFIG_RAMLOG is not set +# CONFIG_SYSLOG_INTBUFFER is not set +# CONFIG_SYSLOG_TIMESTAMP is not set +CONFIG_SYSLOG_SERIAL_CONSOLE=y +# CONFIG_SYSLOG_CHAR is not set +CONFIG_SYSLOG_CONSOLE=y +# CONFIG_SYSLOG_NONE is not set +# CONFIG_SYSLOG_FILE is not set +# CONFIG_SYSLOG_CHARDEV is not set + +# +# Networking Support +# +# CONFIG_ARCH_HAVE_NET is not set +# CONFIG_ARCH_HAVE_PHY is not set +# CONFIG_NET is not set + +# +# Crypto API +# +# CONFIG_CRYPTO is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_AUTOMOUNTER is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +CONFIG_FS_READABLE=y +CONFIG_FS_WRITABLE=y +# CONFIG_FS_AIO is not set +# CONFIG_FS_NAMED_SEMAPHORES is not set +CONFIG_FS_MQUEUE_MPATH="/var/mqueue" +# 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_FORCE_INDIRECT is not set +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_DIRECT_RETRY=y +# CONFIG_FS_NXFFS is not set +CONFIG_FS_ROMFS=y +# CONFIG_FS_TMPFS is not set +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_REGISTER=y + +# +# Exclude individual procfs entries +# +# CONFIG_FS_PROCFS_EXCLUDE_PROCESS is not set +# CONFIG_FS_PROCFS_EXCLUDE_UPTIME is not set +# CONFIG_FS_PROCFS_EXCLUDE_MOUNTS is not set +# CONFIG_FS_UNIONFS is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=1 +# CONFIG_ARCH_HAVE_HEAP2 is not set +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Wireless Support +# + +# +# Binary Loader +# +# 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_LIBC_LONG_LONG=y +# CONFIG_LIBC_IOCTL_VARIADIC is not set +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_LIBC_TMPDIR="/tmp" +CONFIG_LIBC_MAX_TMPFILE=32 +CONFIG_ARCH_LOWPUTC=y +# CONFIG_LIBC_LOCALTIME is not set +CONFIG_TIME_EXTENDED=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 +CONFIG_ARCH_HAVE_TLS=y +# CONFIG_TLS is not set +# CONFIG_LIBC_NETDB is not set +# CONFIG_NETDB_HOSTFILE is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_CRC64_FAST is not set +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set +# CONFIG_LIB_HEX2BIN 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 + +# +# CAN Utilities +# + +# +# Examples +# +# CONFIG_EXAMPLES_CHAT is not set +# CONFIG_EXAMPLES_CONFIGDATA is not set +# CONFIG_EXAMPLES_CPUHOG is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FSTEST 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_HIDKBD is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_MEDIA is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MOUNT_BLOCKDEVICE is not set +CONFIG_EXAMPLES_MOUNT_NSECTORS=2048 +CONFIG_EXAMPLES_MOUNT_SECTORSIZE=512 +CONFIG_EXAMPLES_MOUNT_RAMDEVNO=0 +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NSH_CXXINITIALIZE is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTERM is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PCA9635 is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_PPPD is not set +# CONFIG_EXAMPLES_RFID_READUID is not set +# CONFIG_EXAMPLES_RGBLED is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERIALBLASTER is not set +# CONFIG_EXAMPLES_SERIALRX is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMP 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_UNIONFS is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set +# CONFIG_EXAMPLES_WEBSERVER is not set + +# +# File System Utilities +# +# CONFIG_FSUTILS_INIFILE is not set +# CONFIG_FSUTILS_PASSWD is not set + +# +# GPS Utilities +# +# CONFIG_GPSUTILS_MINMEA_LIB is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set +# CONFIG_GRAPHICS_TRAVELER is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_BAS is not set +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_MICROPYTHON is not set +# CONFIG_INTERPRETERS_MINIBASIC is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# Network Utilities +# +# CONFIG_NETUTILS_CHAT is not set +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_ESP8266 is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_THTTPD is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +# CONFIG_NSH_MOTD is not set + +# +# Command Line Configuration +# +CONFIG_NSH_READLINE=y +# CONFIG_NSH_CLE is not set +CONFIG_NSH_LINELEN=128 +# CONFIG_NSH_DISABLE_SEMICOLON is not set +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +CONFIG_NSH_DISABLE_ADDROUTE=y +CONFIG_NSH_DISABLE_BASENAME=y +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_CMP is not set +# CONFIG_NSH_DISABLE_DATE is not set +CONFIG_NSH_DISABLE_DD=y +# CONFIG_NSH_DISABLE_DF is not set +CONFIG_NSH_DISABLE_DELROUTE=y +CONFIG_NSH_DISABLE_DIRNAME=y +# 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=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +# CONFIG_NSH_DISABLE_KILL is not set +CONFIG_NSH_DISABLE_LOSETUP=y +CONFIG_NSH_DISABLE_LOSMART=y +# CONFIG_NSH_DISABLE_LS is not set +CONFIG_NSH_DISABLE_MB=y +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +CONFIG_NSH_DISABLE_MKFIFO=y +CONFIG_NSH_DISABLE_MKRD=y +CONFIG_NSH_DISABLE_MH=y +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_MW is not set +CONFIG_NSH_DISABLE_PRINTF=y +# CONFIG_NSH_DISABLE_PS is not set +CONFIG_NSH_DISABLE_PSSTACKUSAGE=y +CONFIG_NSH_DISABLE_PUT=y +# 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_TIME is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +CONFIG_NSH_DISABLE_UNAME=y +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_NSH_DISABLE_WGET=y +CONFIG_NSH_DISABLE_XD=y +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=0 + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_PROC_MOUNTPOINT="/proc" +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y + +# +# Scripting Support +# +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS 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_DEFAULTROMFS is not set +CONFIG_NSH_ARCHROMFS=y +# CONFIG_NSH_CUSTOMROMFS is not set +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" + +# +# Console Configuration +# +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set +# CONFIG_NSH_ALTCONDEV is not set +CONFIG_NSH_ARCHINIT=y +# CONFIG_NSH_LOGIN is not set +# CONFIG_NSH_CONSOLE_LOGIN is not set + +# +# NxWidgets/NxWM +# + +# +# Platform-specific Support +# +# CONFIG_PLATFORM_CONFIGDATA is not set + +# +# System Libraries and NSH Add-Ons +# +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CDCACM_DEVMINOR=0 +# CONFIG_SYSTEM_CLE is not set +CONFIG_SYSTEM_CUTERM=y +CONFIG_SYSTEM_CUTERM_DEFAULT_DEVICE="/dev/ttyS0" +CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600 +CONFIG_SYSTEM_CUTERM_STACKSIZE=2048 +CONFIG_SYSTEM_CUTERM_PRIORITY=100 +# CONFIG_SYSTEM_FREE is not set +# CONFIG_SYSTEM_HEX2BIN is not set +# CONFIG_SYSTEM_HEXED is not set +# CONFIG_SYSTEM_I2CTOOL is not set +# CONFIG_SYSTEM_INSTALL is not set +# CONFIG_SYSTEM_RAMTEST is not set +CONFIG_READLINE_HAVE_EXTMATCH=y +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y +# CONFIG_READLINE_TABCOMPLETION is not set +# CONFIG_READLINE_CMD_HISTORY is not set +# CONFIG_SYSTEM_STACKMONITOR is not set +# CONFIG_SYSTEM_SUDOKU is not set +# CONFIG_SYSTEM_SYSTEM is not set +# CONFIG_SYSTEM_TEE is not set +# CONFIG_SYSTEM_UBLOXMODEM is not set +# CONFIG_SYSTEM_VI is not set +# CONFIG_SYSTEM_ZMODEM is not set diff --git a/nuttx-configs/nxphlite-v1/nsh/setenv.sh b/nuttx-configs/nxphlite-v1/nsh/setenv.sh new file mode 100755 index 0000000000..fba50358f6 --- /dev/null +++ b/nuttx-configs/nxphlite-v1/nsh/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# nuttx-configs/nxphlite-v1/nsh/setenv.sh +# +# Copyright (C) 2013 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 is 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 is 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" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is 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/nxphlite-v1/scripts/ld.script b/nuttx-configs/nxphlite-v1/scripts/ld.script new file mode 100644 index 0000000000..350040c291 --- /dev/null +++ b/nuttx-configs/nxphlite-v1/scripts/ld.script @@ -0,0 +1,159 @@ +/**************************************************************************** + * configs/nxphlite-v1/scripts/flash.ld + * + * Copyright (C) 2016 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 MK64FX512VLQ12 has 512 KiB of FLASH beginning at address 0x0000:0000 and + * 192KiB of SRAM beginning at address 0x1ffe:0000- (SRAM_L) and 0x2000:000 + * (SRAM_U). + * + * NOTE: that the first part of the K64 FLASH region is reserved for + * interrupt vectflash and, following that, is a region from 0x0000:0400 + * to 0x0000:040f that is reserved for the FLASH control fields (FCF). + * + * NOTE: The on-chip RAM is split evenly among SRAM_L and SRAM_U. The RAM is + * also implemented such that the SRAM_L and SRAM_U ranges form a + * contiguous block in the memory map. + */ + +MEMORY +{ + vectflash (rx) : ORIGIN = 0x00000000, LENGTH = 1K + cfmprotect (rx) : ORIGIN = 0x00000400, LENGTH = 16 + progflash (rx) : ORIGIN = 0x00000410, LENGTH = 512K - (1K + 16) + datasram (rwx) : ORIGIN = 0x1fff0000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) +EXTERN(__flashconfigbytes) +SECTIONS +{ + .vectors : { + _svectors = ABSOLUTE(.); + KEEP(*(.vectors)) + _evectors = ABSOLUTE(.); + } > vectflash + + .cfmprotect : { + KEEP(*(.cfmconfig)) + } > cfmprotect + + .text : { + _stext = ABSOLUTE(.); + *(.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; + } > progflash + + /* + * Init functions (static constructors and the like) + */ + + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > progflash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > progflash + + .ARM.extab : { + *(.ARM.extab*) + } > progflash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > progflash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > datasram AT > progflash + + .ramfunc ALIGN(4): { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > datasram AT > progflash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > datasram + + /* 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/nxphlite-v1/src/Makefile b/nuttx-configs/nxphlite-v1/src/Makefile new file mode 100644 index 0000000000..e99d652fa5 --- /dev/null +++ b/nuttx-configs/nxphlite-v1/src/Makefile @@ -0,0 +1,86 @@ +############################################################################ +# +# Copyright (C) 2013 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) + +ifneq ($(BOARD_CONTEXT),y) +context: +endif + +-include Make.dep diff --git a/nuttx-configs/nxphlite-v1/src/empty.c b/nuttx-configs/nxphlite-v1/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/nxphlite-v1/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/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig index a544839b61..aaa7884ef2 100644 --- a/nuttx-configs/tap-v1/nsh/defconfig +++ b/nuttx-configs/tap-v1/nsh/defconfig @@ -63,7 +63,8 @@ CONFIG_ARCH_ARM=y # 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_SIM is not setMMSC + # CONFIG_ARCH_X86 is not set # CONFIG_ARCH_Z16 is not set # CONFIG_ARCH_Z80 is not set diff --git a/src/drivers/boards/common/kinetis/board_reset.c b/src/drivers/boards/common/kinetis/board_reset.c new file mode 100644 index 0000000000..6ef9f51515 --- /dev/null +++ b/src/drivers/boards/common/kinetis/board_reset.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_reset.c + * Implementation of kinetis based Board RESET API + */ + +#include +#include +#include + + +int board_set_bootload_mode(board_reset_e mode) +{ + uint32_t regvalue = 0; + + switch (mode) { + case board_reset_normal: + case board_reset_extended: + break; + + case board_reset_enter_bootloader: + regvalue = 0xb007b007; + break; + + default: + return -EINVAL; + } + +// todo: Add a way to enter bootloader + UNUSED(regvalue); + return OK; +} + + +void board_system_reset(int status) +{ + board_reset(status); + + while (1); +} diff --git a/src/drivers/boards/nxphlite-v1/CMakeLists.txt b/src/drivers/boards/nxphlite-v1/CMakeLists.txt new file mode 100644 index 0000000000..23887f7941 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2016 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__nxphlite-v1 + COMPILE_FLAGS + SRCS + ../common/kinetis/board_reset.c + ../common/board_crashdump.c + ../common/board_dma_alloc.c + nxphlite_autoleds.c + nxphlite_automount.c + nxphlite_bringup.c + nxphlite_can.c + nxphlite_init.c + nxphlite_led.c + nxphlite_pwm.c + nxphlite_sdhc.c + nxphlite_sdio.c + nxphlite_spi.c + nxphlite_timer_config.c + nxphlite_usb.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/nxphlite-v1/board_config.h b/src/drivers/boards/nxphlite-v1/board_config.h new file mode 100644 index 0000000000..5f2c0fd89b --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/board_config.h @@ -0,0 +1,565 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * nxphlite-v1 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include +#include + +/* NXPHLITE GPIOs ***********************************************************************************/ +/* LEDs */ +/* An RGB LED is connected through GPIO as shown below: + * TBD (no makring on schematic) + * LED K64 + * ------ ------------------------------------------------------- + * RED FB_CS3_b/ FB_BE7_0_BLS31_24_b/ FB_TA_b/ ENET0_1588_TMR3/ UART3_CTS_b/ PTC19 + * GREEN LLWU_P12/ FB_CS1_b/ FB_TS_b/ FB_ALE/ FTM3_CH0/ UART2_RTS_b/ SPI0_PCS0/ PTD0 + * BLUE FB_CS0_b/ FTM3_CH1/ UART2_CTS_b/ SPI0_SCK/ ADC0_SE5b/ PTD1 + */ + +#define GPIO_LED_R (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTC | PIN19) +#define GPIO_LED_G (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTD | PIN0) +#define GPIO_LED_B (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTD | PIN1) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 2 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* PPM IN + * + */ + +#define HRT_PPM_CHANNEL 0 /* use capture/compare channel */ +#define GPIO_PPM_IN PIN_FTM2_CH0_1 /* PTA10 220R to P4-1 */ + + +/* IR transmitter & receiver + * + * Rx could be on UART0 RX + * TX is On PTA24 + * Usage TBD - both left as inputs for now. + * + */ +#define GPIO_IR_TRANSMITTER (GPIO_PULLUP | PIN_PORTA | PIN24) +#define GPIO_IR_RRCEIVER (GPIO_PULLUP | PIN_PORTA | PIN21) + +/* SBUS Control + * + * SBUS is brought out on P4-4 and controlled buy the following. + * */ + +#define GPIO_SBUS_EN (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTA | PIN2) +#define GPIO_SBUS_IN /* Not usable 0 needs UART */ (GPIO_PULLUP | PIN_PORTA | PIN8) +#define GPIO_SBUS_OUT /* Not usable 0 needs UART */ (GPIO_INPUT | PIN_PORTA | PIN9) +#define GPIO_RSSI_IN /* Needs s/b on ADC/Timer */ (GPIO_INPUT | PIN_PORTA | PIN25) + +/* Ethernet Control + * + * Unintalized to Reset Disabled and Inhibited + */ + +#define GPIO_E_RST (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTA | PIN28) +#define GPIO_E_EN (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTA | PIN29) +#define GPIO_E_INH (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN2) + + +/* CAN Control + * Control pin S allows two operating modes to be selected: + * high-speed mode (Low) or silent mode (high) + */ + +#define GPIO_CAN0_STB (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN4) + + +/* URF Connector + * TBD + */ + +#define GPIO_ECH (GPIO_PULLUP | PIN_PORTC | PIN8) +#define GPIO_TRI (GPIO_PULLUP | PIN_PORTC | PIN9) + +/* GPIO on UART P10 + * TBD + */ + +#define GPIO_PTD11 (GPIO_PULLUP | PIN_PORTD | PIN11) +#define GPIO_PTD12 (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTD | PIN11) + +/* GPIO on UART P14 + * TBD + */ + +#define GPIO_PTD15 (GPIO_PULLUP | PIN_PORTD | PIN15) +#define GPIO_PTE4 (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTE | PIN4) + +/* Safety Switch + * TBD + */ +#define GPIO_LED_SAFETY (GPIO_HIGHDRIVE | GPIO_OUTPUT_ZER0 | PIN_PORTD | PIN13) +#define GPIO_BTN_SAFETY (GPIO_PULLUP | PIN_PORTD | PIN14) + +/* NXPHlite-v1 GPIOs ****************************************************************/ + +/* SDIO + * + * NOT IN Current HW t uses SPI + * A micro Secure Digital (SD) card slot is available on the FRDM-K64F connected to + * the SD Host Controller (SDHC) signals of the MCU. This slot will accept micro + * format SD memory cards. The SD card detect pin (PTE6) is an open switch that + * shorts with VDD when card is inserted. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal K64F Pin + * ------------ ------------- -------- + * DAT0 SDHC0_D0 PTE0 + * DAT1 SDHC0_D1 PTE1 + * DAT2 SDHC0_D2 PTE5 + * CD/DAT3 SDHC0_D3 PTE4 + * CMD SDHC0_CMD PTE3 + * CLK SDHC0_DCLK PTE2 + * SWITCH D_CARD_DETECT PTE6 + * ------------ ------------- -------- + * + * There is no Write Protect pin available to the K64F Or Card detect. + */ + +/* SPI + * + * SD Card is on SPI 0 + * FXOS8700CQ Accelerometer & Magnetometer is on SPI 1 + * FXAS21002CQ Gyro is on SPI 2 + */ + +/* SPI Bus assignments */ + +#define PX4_SPI_BUS_SDCARD 0 +#define PX4_SPI_BUS_ACCEL_MAG 1 +#define PX4_SPI_BUS_GYRO 2 + + +/* SPI chip selects */ + +#define GPIO_SPI_CS_FXAS21002CQ_GYRO (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN20) +#define GPIO_SPI_CS_FXOS8700CQ_ACCEL_MAG (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN10) +#define GPIO_SPI_CS_SDCARD (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTC | PIN4) + +/* SPI device reset signals + * In Inactive state + */ + +#define GPIO_GM_RST (GPIO_LOWDRIVE | GPIO_OUTPUT_ZER0 | PIN_PORTC | PIN0) +#define GPIO_A_RST (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN7) + +/* Sensor interrupts */ + +#define GPIO_EXTI_GYRO_INT1 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTC | PIN1) +#define GPIO_EXTI_GYRO_INT2 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTC | PIN2) +#define GPIO_EXTI_ACCEL_MAG_INT1 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTB | PIN8) +#define GPIO_EXTI_ACCEL_MAG_INT2 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTB | PIN9) +#define GPIO_EXTI_BARO_INT1 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTE | PIN26) +#define GPIO_EXTI_BARO_INT2 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTE | PIN27) + +#define PX4_MK_SPI_SEL(b,d) ((((b) & 0xf) << 4) + ((d) & 0xf)) +#define PX4_SPI_BUS_ID(bd) (((bd) >> 4) & 0xf) +#define PX4_SPI_DEV_ID(bd) ((bd) & 0xf) + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ + +#define PX4_SPIDEV_SDCARD PX4_MK_SPI_SEL(PX4_SPI_BUS_SDCARD,0) +#define PX4_SDCARD_BUS_CS_GPIO {GPIO_SPI_CS_SDCARD} +#define PX4_SDCARD_BUS_FIRST_CS PX4_SPIDEV_SDCARD +#define PX4_SDCARD_BUS_LAST_CS PX4_SPIDEV_SDCARD + +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_ACCEL_MAG,0) +#define PX4_ACCEL_MAG_BUS_CS_GPIO {GPIO_SPI_CS_FXOS8700CQ_ACCEL_MAG} +#define PX4_ACCEL_MAG_BUS_FIRST_CS PX4_SPIDEV_ACCEL_MAG +#define PX4_ACCEL_MAG_BUS_LAST_CS PX4_SPIDEV_ACCEL_MAG + +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_GYRO,0) +#define PX4_GYRO_BUS_CS_GPIO {GPIO_SPI_CS_FXAS21002CQ_GYRO} +#define PX4_GYRO_BUS_FIRST_CS PX4_SPIDEV_GYRO +#define PX4_GYRO_BUS_LAST_CS PX4_SPIDEV_GYRO + + +/* I2C busses */ + +#define PX4_I2C_BUS_ONBOARD 1 +#define PX4_I2C_BUS_EXPANSION 2 + +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_LIS3MDL 0x1e + +/* + * 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 << 0 // TBD + +// ADC defines to be used in sensors.cpp to read from a particular channel + +// BAT_VSENS ADC1_SE16 or ADC0_SE22 +// BAT_ISENS ADC0_SE16 or ADC0_SE21 +// V5_VSENS ADC1_SE18 + +// AD1 ADC1_SE5a +// AD2 ADC1_SE7a +// AD3 ADC0_SE10 +// AD4 ADC1_SE12 + +#define ADC_BATTERY_VOLTAGE_CHANNEL 22 +#define ADC_BATTERY_CURRENT_CHANNEL 21 +#define ADC_5V_RAIL_SENSE 18 + +#define BOARD_BATTERY1_V_DIV (10.177939394f) +#define BOARD_BATTERY1_A_PER_V (15.391030303f) + +/* User GPIOs + * + * GPIO- + * Define as GPIO input / GPIO outputs and timers IO + */ + +#define PX4_MK_GPIO(pin_ftmx, io) ((pin_ftmx) & (((uint32_t) ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) | (uint32_t)(io))) +#define PX4_MK_GPIO_INPUT(pin_ftmx) PX4_MK_GPIO(pin_ftmx, GPIO_PULLUP) +#define PX4_MK_GPIO_OUTPUT(pin_ftmx) PX4_MK_GPIO(pin_ftmx, GPIO_HIGHDRIVE) + +#define GPIO_GPIO0_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH0OUT) +#define GPIO_GPIO1_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH1OUT) +#define GPIO_GPIO2_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH2OUT) +#define GPIO_GPIO3_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH3OUT) +#define GPIO_GPIO4_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH4OUT) +#define GPIO_GPIO5_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH5OUT) +#define GPIO_GPIO6_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH6OUT) +#define GPIO_GPIO7_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM3_CH0OUT) +#define GPIO_GPIO8_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH4OUT) +#define GPIO_GPIO9_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH5OUT) +#define GPIO_GPIO10_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH6OUT) +#define GPIO_GPIO11_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH7OUT) +#define GPIO_GPIO12_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH3OUT) +#define GPIO_GPIO13_INPUT PX4_MK_GPIO_INPUT(GPIO_FTM0_CH2OUT) + +#define GPIO_GPIO0_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH0OUT) +#define GPIO_GPIO1_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH1OUT) +#define GPIO_GPIO2_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH2OUT) +#define GPIO_GPIO3_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH3OUT) +#define GPIO_GPIO4_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH4OUT) +#define GPIO_GPIO5_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH5OUT) +#define GPIO_GPIO6_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH6OUT) +#define GPIO_GPIO7_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM3_CH0OUT) +#define GPIO_GPIO8_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH4OUT) +#define GPIO_GPIO9_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH5OUT) +#define GPIO_GPIO10_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH6OUT) +#define GPIO_GPIO11_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH7OUT) +#define GPIO_GPIO12_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH3OUT) +#define GPIO_GPIO13_OUTPUT PX4_MK_GPIO_OUTPUT(GPIO_FTM0_CH2OUT) + +/* Timer I/O PWM and capture + * + * 14 PWM outputs are configured. + * 14 Timer inputs are configured. + * + * Pins: + * Defined in board.h + */ +// todo:Desine this! + +#define DIRECT_PWM_OUTPUT_CHANNELS 14 +#define DIRECT_INPUT_TIMER_CHANNELS 14 + +/* Power supply control and monitoring GPIOs */ +// None + +#define GPIO_PERIPH_3V3_EN 0 + +#define GPIO_SBUS_INV 0 +#define INVERT_RC_INPUT(_s) px4_arch_gpiowrite(GPIO_SBUS_INV, _s) + + +/* Tone alarm output is On E28 */ +#define TONE_ALARM_TIMER 2 /* timer */ +#define TONE_ALARM_CHANNEL 3 /* channel */ +#define GPIO_TONE_ALARM_IDLE (GPIO_LOWDRIVE | GPIO_OUTPUT_ZER0 | PIN_PORTE | PIN28) +#define GPIO_TONE_ALARM (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTE | PIN28) + +/* USB + * + * Note No external VBUD detection + */ + + +/* PWM input driver. Use FMU PWM14 pin + * todo:desing this + */ +#define PWMIN_TIMER 0 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_FTM0_CH2IN + +#define BOARD_NAME "NXPHLITE_V1" + +/* Not Supported in V1 HW + * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (0) +#define BOARD_ADC_BRICK_VALID (1) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_FMU_GPIO_TAB { \ + {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}, \ + {GPIO_GPIO13_INPUT, GPIO_GPIO13_OUTPUT, 0}, \ + } +/* + * GPIO numbers. + * + * There are no alternate functions on this board. + */ +#define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +#define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +#define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +#define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +#define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +#define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ +#define GPIO_SERVO_7 (1<<6) /**< servo 7 output */ +#define GPIO_SERVO_8 (1<<7) /**< servo 8 output */ +#define GPIO_SERVO_9 (1<<8) /**< servo 9 output */ +#define GPIO_SERVO_10 (1<<9) /**< servo 9 output */ +#define GPIO_SERVO_11 (1<<10) /**< servo 10 output */ +#define GPIO_SERVO_12 (1<<11) /**< servo 11 output */ +#define GPIO_SERVO_13 (1<<12) /**< servo 12 output */ +#define GPIO_SERVO_14 (1<<13) /**< servo 13 output */ + +/* This board provides a DMA pool and APIs */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + + +/************************************************************************************ + * Public data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: nxphlite_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXPHlite-v1 board. + * + ************************************************************************************/ + +void nxphlite_spidev_initialize(void); + +/************************************************************************************ + * Name: nxphlite_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +int nxphlite_spi_bus_initialize(void); + +/**************************************************************************************************** + * Name: board_spi_reset board_peripheral_reset + * + * Description: + * Called to reset SPI and the perferal bus + * + ****************************************************************************************************/ +void board_spi_reset(int ms); +void board_peripheral_reset(int ms); + +/************************************************************************************ + * Name: nxphlite_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the NXPHlite-v1 board. + * + ************************************************************************************/ + +void nxphlite_usbinitialize(void); + +/************************************************************************************ + * Name: nxphlite_bringup + * + * Description: + * Bring up board features + * + ************************************************************************************/ + +#if defined(CONFIG_LIB_BOARDCTL) || defined(CONFIG_BOARD_INITIALIZE) +int nxphlite_bringup(void); +#endif + +/**************************************************************************** + * Name: nxphlite_sdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +#ifdef HAVE_MMCSD +int nxphlite_sdhc_initialize(void); +#else +# define nxphlite_sdhc_initialize() (OK) +#endif + +/**************************************************************************** + * Name: nxphlite_sdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int nxphlite_sdio_initialize(void); + +/************************************************************************************ + * Name: nxphlite_cardinserted + * + * Description: + * Check if a card is inserted into the SDHC slot + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +bool nxphlite_cardinserted(void); +#else +# define nxphlite_cardinserted() (false) +#endif + +/************************************************************************************ + * Name: nxphlite_writeprotected + * + * Description: + * Check if the card in the MMC/SD slot is write protected + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +bool nxphlite_writeprotected(void); +#else +# define nxphlite_writeprotected() (false) +#endif + +/************************************************************************************ + * Name: nxphlite_automount_initialize + * + * Description: + * Configure auto-mounter for the configured SDHC slot + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +void nxphlite_automount_initialize(void); +#endif + +/************************************************************************************ + * Name: nxphlite_automount_event + * + * Description: + * The SDHC card detection logic has detected an insertion or removal event. It + * has already scheduled the MMC/SD block driver operations. Now we need to + * schedule the auto-mount event which will occur with a substantial delay to make + * sure that everything has settle down. + * + * Input Parameters: + * inserted - True if the card is inserted in the slot. False otherwise. + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled. + * + ************************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +void nxphlite_automount_event(bool inserted); +#endif + +#include "../common/board_common.h" + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_autoleds.c b/src/drivers/boards/nxphlite-v1/nxphlite_autoleds.c new file mode 100644 index 0000000000..063ef60971 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_autoleds.c @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * + * 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. + * + ****************************************************************************/ +/* + * This module shaol be used during board bring up of Nuttx. + * + * The NXPHlite-v1 has a separate Red, Green and Blue LEDs driven by the K64F + * as follows: + * + * LED K64 + * ------ ------------------------------------------------------- + * RED FB_CS3_b/ FB_BE7_0_BLS31_24_b/ FB_TA_b/ ENET0_1588_TMR3/ UART3_CTS_b/ PTC19 + * GREEN LLWU_P12/ FB_CS1_b/ FB_TS_b/ FB_ALE/ FTM3_CH0/ UART2_RTS_b/ SPI0_PCS0/ PTD0 + * BLUE FB_CS0_b/ FTM3_CH1/ UART2_CTS_b/ SPI0_SCK/ ADC0_SE5b/ PTD1 + * + * If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the Freedom K64F. The following definitions describe how NuttX controls + * the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ------------------- ----------------------- ----------------- + * LED_STARTED NuttX has been started OFF OFF OFF + * LED_HEAPALLOCATE Heap has been allocated OFF OFF ON + * LED_IRQSENABLED Interrupts enabled OFF OFF ON + * LED_STACKCREATED Idle stack created OFF ON OFF + * LED_INIRQ In an interrupt (no change) + * LED_SIGNAL In a signal handler (no change) + * LED_ASSERTION An assertion failed (no change) + * LED_PANIC The system has crashed FLASH OFF OFF + * LED_IDLE K64 is in sleep mode (Optional, not used) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "kinetis.h" +#include "board_config.h" + +#ifdef CONFIG_ARCH_LEDS + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Summary of all possible settings */ + +#define LED_NOCHANGE 0 /* LED_IRQSENABLED, LED_INIRQ, LED_SIGNAL, LED_ASSERTION */ +#define LED_OFF_OFF_OFF 1 /* LED_STARTED */ +#define LED_OFF_OFF_ON 2 /* LED_HEAPALLOCATE */ +#define LED_OFF_ON_OFF 3 /* LED_STACKCREATED */ +#define LED_ON_OFF_OFF 4 /* LED_PANIC */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + * + * Description: + * Initialize the on-board LED + * + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + kinetis_pinconfig(GPIO_LED_R); + kinetis_pinconfig(GPIO_LED_G); + kinetis_pinconfig(GPIO_LED_B); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (led != LED_NOCHANGE) { + bool redoff = true; + bool greenoff = true; + bool blueoff = true; + + switch (led) { + default: + case LED_OFF_OFF_OFF: + break; + + case LED_OFF_OFF_ON: + blueoff = false; + break; + + case LED_OFF_ON_OFF: + greenoff = false; + break; + + case LED_ON_OFF_OFF: + redoff = false; + break; + } + + kinetis_gpiowrite(GPIO_LED_R, redoff); + kinetis_gpiowrite(GPIO_LED_G, greenoff); + kinetis_gpiowrite(GPIO_LED_B, blueoff); + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (led == LED_ON_OFF_OFF) { + kinetis_gpiowrite(GPIO_LED_R, true); + kinetis_gpiowrite(GPIO_LED_G, true); + kinetis_gpiowrite(GPIO_LED_B, true); + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_automount.c b/src/drivers/boards/nxphlite-v1/nxphlite_automount.c new file mode 100644 index 0000000000..f2a0202971 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_automount.c @@ -0,0 +1,314 @@ +/************************************************************************************ + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#if defined(CONFIG_FS_AUTOMOUNTER_DEBUG) && !defined(CONFIG_DEBUG_FS) +# define CONFIG_DEBUG_FS 1 +#endif + +#include + +#include +#include +#include + +#include "board_config.h" + +#ifdef HAVE_AUTOMOUNTER + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +#ifndef NULL +# define NULL (FAR void *)0 +#endif + +#ifndef OK +# define OK 0 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This structure represents the changeable state of the automounter */ + +struct nxphlite_automount_state_s { + volatile automount_handler_t handler; /* Upper half handler */ + FAR void *arg; /* Handler argument */ + bool enable; /* Fake interrupt enable */ + bool pending; /* Set if there an event while disabled */ +}; + +/* This structure represents the static configuration of an automounter */ + +struct nxphlite_automount_config_s { + /* This must be first thing in structure so that we can simply cast from struct + * automount_lower_s to struct nxphlite_automount_config_s + */ + + struct automount_lower_s lower; /* Publicly visible part */ + FAR struct nxphlite_automount_state_s *state; /* Changeable state */ +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +static int nxphlite_attach(FAR const struct automount_lower_s *lower, + automount_handler_t isr, FAR void *arg); +static void nxphlite_enable(FAR const struct automount_lower_s *lower, bool enable); +static bool nxphlite_inserted(FAR const struct automount_lower_s *lower); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +static struct nxphlite_automount_state_s g_sdhc_state; +static const struct nxphlite_automount_config_s g_sdhc_config = { + .lower = + { + .fstype = CONFIG_NXPHLITE_SDHC_AUTOMOUNT_FSTYPE, + .blockdev = CONFIG_NXPHLITE_SDHC_AUTOMOUNT_BLKDEV, + .mountpoint = CONFIG_NXPHLITE_SDHC_AUTOMOUNT_MOUNTPOINT, + .ddelay = MSEC2TICK(CONFIG_NXPHLITE_SDHC_AUTOMOUNT_DDELAY), + .udelay = MSEC2TICK(CONFIG_NXPHLITE_SDHC_AUTOMOUNT_UDELAY), + .attach = nxphlite_attach, + .enable = nxphlite_enable, + .inserted = nxphlite_inserted + }, + .state = &g_sdhc_state +}; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: nxphlite_attach + * + * Description: + * Attach a new SDHC event handler + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * isr - The new event handler to be attach + * arg - Client data to be provided when the event handler is invoked. + * + * Returned Value: + * Always returns OK + * + ************************************************************************************/ + +static int nxphlite_attach(FAR const struct automount_lower_s *lower, + automount_handler_t isr, FAR void *arg) +{ + FAR const struct nxphlite_automount_config_s *config; + FAR struct nxphlite_automount_state_s *state; + + /* Recover references to our structure */ + + config = (FAR struct nxphlite_automount_config_s *)lower; + DEBUGASSERT(config != NULL && config->state != NULL); + + state = config->state; + + /* Save the new handler info (clearing the handler first to eliminate race + * conditions). + */ + + state->handler = NULL; + state->pending = false; + state->arg = arg; + state->handler = isr; + return OK; +} + +/************************************************************************************ + * Name: nxphlite_enable + * + * Description: + * Enable card insertion/removal event detection + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * enable - True: enable event detection; False: disable + * + * Returned Value: + * None + * + ************************************************************************************/ + +static void nxphlite_enable(FAR const struct automount_lower_s *lower, bool enable) +{ + FAR const struct nxphlite_automount_config_s *config; + FAR struct nxphlite_automount_state_s *state; + irqstate_t flags; + + /* Recover references to our structure */ + + config = (FAR struct nxphlite_automount_config_s *)lower; + DEBUGASSERT(config != NULL && config->state != NULL); + + state = config->state; + + /* Save the fake enable setting */ + + flags = enter_critical_section(); + state->enable = enable; + + /* Did an interrupt occur while interrupts were disabled? */ + + if (enable && state->pending) { + /* Yes.. perform the fake interrupt if the interrutp is attached */ + + if (state->handler) { + bool inserted = nxphlite_cardinserted(); + (void)state->handler(&config->lower, state->arg, inserted); + } + + state->pending = false; + } + + leave_critical_section(flags); +} + +/************************************************************************************ + * Name: nxphlite_inserted + * + * Description: + * Check if a card is inserted into the slot. + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * + * Returned Value: + * True if the card is inserted; False otherwise + * + ************************************************************************************/ + +static bool nxphlite_inserted(FAR const struct automount_lower_s *lower) +{ + return nxphlite_cardinserted(); +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: nxphlite_automount_initialize + * + * Description: + * Configure auto-mounters for each enable and so configured SDHC + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ************************************************************************************/ + +void nxphlite_automount_initialize(void) +{ + FAR void *handle; + + finfo("Initializing automounter(s)\n"); + + /* Initialize the SDHC0 auto-mounter */ + + handle = automount_initialize(&g_sdhc_config.lower); + + if (!handle) { + ferr("ERROR: Failed to initialize auto-mounter for SDHC0\n"); + } +} + +/************************************************************************************ + * Name: nxphlite_automount_event + * + * Description: + * The SDHC card detection logic has detected an insertion or removal event. It + * has already scheduled the MMC/SD block driver operations. Now we need to + * schedule the auto-mount event which will occur with a substantial delay to make + * sure that everything has settle down. + * + * Input Parameters: + * slotno - Identifies the SDHC0 slot: SDHC0_SLOTNO or SDHC1_SLOTNO. There is a + * terminology problem here: Each SDHC supports two slots, slot A and slot B. + * Only slot A is used. So this is not a really a slot, but an HSCMI peripheral + * number. + * inserted - True if the card is inserted in the slot. False otherwise. + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled. + * + ************************************************************************************/ + +void nxphlite_automount_event(bool inserted) +{ + FAR const struct nxphlite_automount_config_s *config = &g_sdhc_config; + FAR struct nxphlite_automount_state_s *state = &g_sdhc_state; + + /* Is the auto-mounter interrupt attached? */ + + if (state->handler) { + /* Yes.. Have we been asked to hold off interrupts? */ + + if (!state->enable) { + /* Yes.. just remember the there is a pending interrupt. We will + * deliver the interrupt when interrupts are "re-enabled." + */ + + state->pending = true; + + } else { + /* No.. forward the event to the handler */ + + (void)state->handler(&config->lower, state->arg, inserted); + } + } +} + +#endif /* HAVE_AUTOMOUNTER */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_bringup.c b/src/drivers/boards/nxphlite-v1/nxphlite_bringup.c new file mode 100644 index 0000000000..1eabecc9e4 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_bringup.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include "board_config.h" + +#if defined(CONFIG_LIB_BOARDCTL) || defined(CONFIG_BOARD_INITIALIZE) + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxphlite_bringup + * + * Description: + * Bring up board features + * + ****************************************************************************/ + +int nxphlite_bringup(void) +{ + int ret; + +#ifdef HAVE_PROC + /* Mount the proc filesystem */ + + syslog(LOG_INFO, "Mounting procfs to /proc\n"); + + ret = mount(NULL, PROCFS_MOUNTPOUNT, "procfs", 0, NULL); + + if (ret < 0) { + syslog(LOG_ERR, + "ERROR: Failed to mount the PROC filesystem: %d (%d)\n", + ret, errno); + return ret; + } + +#endif + +#ifdef HAVE_MMCSD + /* Initialize the SDHC driver */ + + ret = nxphlite_sdhc_initialize(); + + if (ret < 0) { + mcerr("ERROR: nxphlite_sdhc_initialize() failed: %d\n", ret); + } + +#ifdef CONFIG_NXPHLITE_SDHC_MOUNT + + else { + /* REVISIT: A delay seems to be required here or the mount will fail. */ + /* Mount the volume on HSMCI0 */ + + ret = mount(CONFIG_NXPHLITE_SDHC_MOUNT_BLKDEV, + CONFIG_NXPHLITE_SDHC_MOUNT_MOUNTPOINT, + CONFIG_NXPHLITE_SDHC_MOUNT_FSTYPE, + 0, NULL); + + if (ret < 0) { + mcerr("ERROR: Failed to mount %s: %d\n", + CONFIG_NXPHLITE_SDHC_MOUNT_MOUNTPOINT, errno); + } + } + +#endif /* CONFIG_NXPHLITE_SDHC_MOUNT */ +#endif /* HAVE_MMCSD */ + +#ifdef HAVE_AUTOMOUNTER + /* Initialize the auto-mounter */ + + nxphlite_automount_initialize(); +#endif + + UNUSED(ret); + return OK; +} + +#endif /* CONFIG_LIB_BOARDCTL CONFIG_BOARD_INITIALIZE */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_can.c b/src/drivers/boards/nxphlite-v1/nxphlite_can.c new file mode 100644 index 0000000000..832b69cb9d --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_can.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file nxphlite_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include +#include +#include "up_arch.h" + +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(KINETIS_FLEXCAN0) && defined(KINETIS_FLEXCAN1) +# warning "Both CAN0 and CAN1 are enabled. Assuming only CAN0." +# undef KINETIS_FLEXCAN0 +#endif + +#ifdef KINETIS_FLEXCAN0 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All 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 kinetis_caninitialize() to get an instance of the CAN interface */ + + can = kinetis_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("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) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_init.c b/src/drivers/boards/nxphlite-v1/nxphlite_init.c new file mode 100644 index 0000000000..ba5665e99f --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_init.c @@ -0,0 +1,479 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file nxphlite_init.c + * + * NXPHLITEV1v2-specific early startup code. This file implements the + * board_app_initialize() 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 initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" + +#include + +#include +#include + +#include +#include +#include +#include + +#if defined(CONFIG_KINETIS_BBSRAM) //fixme:Need BBSRAM +#include +#endif + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) syslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message syslog +# 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 + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All Kinetis 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 +kinetis_boardinitialize(void) +{ + /* configure LEDs */ + board_autoled_initialize(); + + /* configure ADC pins */ +#if defined(GPIO_ADC1_IN2) + kinetis_pinconfig(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + kinetis_pinconfig(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + kinetis_pinconfig(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + kinetis_pinconfig(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + kinetis_pinconfig(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + kinetis_pinconfig(GPIO_ADC1_IN15); /* PRESSURE_SENS */ +#endif +#if defined(GPIO_VDD_5V_PERIPH_EN) + /* configure power supply control/sense pins */ + kinetis_pinconfig(GPIO_VDD_5V_PERIPH_EN); + kinetis_pinconfig(GPIO_VDD_3V3_SENSORS_EN); + kinetis_pinconfig(GPIO_VDD_BRICK_VALID); + kinetis_pinconfig(GPIO_VDD_SERVO_VALID); + kinetis_pinconfig(GPIO_VDD_5V_HIPOWER_OC); + kinetis_pinconfig(GPIO_VDD_5V_PERIPH_OC); +#endif + /* configure the GPIO pins to outputs and keep them low */ + kinetis_pinconfig(GPIO_GPIO0_OUTPUT); + kinetis_pinconfig(GPIO_GPIO1_OUTPUT); + kinetis_pinconfig(GPIO_GPIO2_OUTPUT); + kinetis_pinconfig(GPIO_GPIO3_OUTPUT); + kinetis_pinconfig(GPIO_GPIO4_OUTPUT); + kinetis_pinconfig(GPIO_GPIO5_OUTPUT); + kinetis_pinconfig(GPIO_GPIO6_OUTPUT); + kinetis_pinconfig(GPIO_GPIO7_OUTPUT); + kinetis_pinconfig(GPIO_GPIO8_OUTPUT); + kinetis_pinconfig(GPIO_GPIO9_OUTPUT); + kinetis_pinconfig(GPIO_GPIO10_OUTPUT); + kinetis_pinconfig(GPIO_GPIO11_OUTPUT); + kinetis_pinconfig(GPIO_GPIO12_OUTPUT); + kinetis_pinconfig(GPIO_GPIO13_OUTPUT); + + /* configure SPI interfaces */ + nxphlite_spidev_initialize(); +} + +// FIXME:STUBS +#include + +int cfsetspeed(FAR struct termios *termiosp, speed_t speed); +int cfsetspeed(FAR struct termios *termiosp, speed_t speed) +{ + return 0; +} + +int up_rtc_getdatetime(FAR struct tm *tp); +int up_rtc_getdatetime(FAR struct tm *tp) +{ + tp->tm_sec = 0; + tp->tm_min = 0; + tp->tm_hour = 0; + tp->tm_mday = 30; + tp->tm_mon = 10; + tp->tm_year = 116; + tp->tm_wday = 1; /* Day of the week (0-6) */ + tp->tm_yday = 0; /* Day of the year (0-365) */ + tp->tm_isdst = 0; /* Non-0 if daylight savings time is in effect */ + return 0; +} + +FAR struct spi_dev_s *kinetis_spibus_initialize(int bus); +FAR struct spi_dev_s *kinetis_spibus_initialize(int bus) +{ + return NULL; +} +static void kinetis_serial_dma_poll(void) +{ + // todo:Stubbed +} +struct termios; +int tcgetattr(int fd, FAR struct termios *termiosp); +int tcsetattr(int fd, int options, FAR const struct termios *termiosp); +int tcgetattr(int fd, FAR struct termios *termiosp) +{ + return -1; +} +int tcsetattr(int fd, int options, FAR const struct termios *termiosp) +{ + return -1; + +} + + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + + /* run C++ ctors before we go any further */ + + up_cxxinitialize(); + +# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) +# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. +# endif + +#else +# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. +#endif + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + message("DMA alloc FAILED"); + } + + /* 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)kinetis_serial_dma_poll, + NULL); + +#if defined(CONFIG_KINETIS_BBSRAM) + + /* NB. the use of the console requires the hrt running + * to poll the DMA + */ + + /* Using Battery Backed Up SRAM */ + + int filesizes[CONFIG_KINETIS_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES; + + stm32_bbsraminitialize(BBSRAM_PATH, filesizes); + +#if defined(CONFIG_KINETIS_SAVE_CRASHDUMP) + + /* Panic Logging in Battery Backed Up Files */ + + /* + * In an ideal world, if a fault happens in flight the + * system save it to BBSRAM will then reboot. Upon + * rebooting, the system will log the fault to disk, recover + * the flight state and continue to fly. But if there is + * a fault on the bench or in the air that prohibit the recovery + * or committing the log to disk, the things are too broken to + * fly. So the question is: + * + * Did we have a hard fault and not make it far enough + * through the boot sequence to commit the fault data to + * the SD card? + */ + + /* Do we have an uncommitted hard fault in BBSRAM? + * - this will be reset after a successful commit to SD + */ + int hadCrash = hardfault_check_status("boot"); + + if (hadCrash == OK) { + + message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \ + " while booting to halt the system!\n"); + + /* Yes. So add one to the boot count - this will be reset after a successful + * commit to SD + */ + + int reboots = hardfault_increment_reboot("boot", false); + + /* Also end the misery for a user that holds for a key down on the console */ + + int bytesWaiting; + ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting)); + + if (reboots > 2 || bytesWaiting != 0) { + + /* Since we can not commit the fault dump to disk. Display it + * to the console. + */ + + hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); + + message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", + reboots, + (bytesWaiting == 0 ? "" : " Due to Key Press\n")); + + + /* For those of you with a debugger set a break point on up_assert and + * then set dbgContinue = 1 and go. + */ + + /* Clear any key press that got us here */ + + static volatile bool dbgContinue = false; + int c = '>'; + + while (!dbgContinue) { + + switch (c) { + + case EOF: + + + case '\n': + case '\r': + case ' ': + continue; + + default: + + putchar(c); + putchar('\n'); + + switch (c) { + + case 'D': + case 'd': + hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); + break; + + case 'C': + case 'c': + hardfault_rearm("boot"); + hardfault_increment_reboot("boot", true); + break; + + case 'B': + case 'b': + dbgContinue = true; + break; + + default: + break; + } // Inner Switch + + message("\nEnter B - Continue booting\n" \ + "Enter C - Clear the fault log\n" \ + "Enter D - Dump fault log\n\n?>"); + fflush(stdout); + + if (!dbgContinue) { + c = getchar(); + } + + break; + + } // outer switch + } // for + + } // inner if + } // outer if + +#endif // CONFIG_KINETIS_SAVE_CRASHDUMP +#endif // CONFIG_KINETIS_BBSRAM + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + /* Configure SPI-based devices */ + +#ifdef CONFIG_SPI + int ret = nxphlite_spi_bus_initialize(); + + if (ret != OK) { + board_autoled_on(LED_RED); + return ret; + } + +#endif + +#ifdef CONFIG_MMCSD + ret = nxphlite_sdio_initialize(); + + if (ret != OK) { + board_autoled_on(LED_RED); + return ret; + } + +#endif + + nxphlite_usbinitialize(); + + return OK; +} diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_led.c b/src/drivers/boards/nxphlite-v1/nxphlite_led.c new file mode 100644 index 0000000000..2ccbad627d --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_led.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file nxphlite_led.c + * + * NXPHLITEV1 LED backend. + */ + +#include + +#include + +#include "kinetis.h" +#include "chip.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 + + +static uint32_t g_ledmap[] = { + GPIO_LED_B, // Indexed by LED_BLUE + GPIO_LED_R, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_G, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + kinetis_pinconfig(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive High to switch on */ + + kinetis_gpiowrite(g_ledmap[led], state); +} + +static bool phy_get_led(int led) +{ + + return kinetis_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_pwm.c b/src/drivers/boards/nxphlite-v1/nxphlite_pwm.c new file mode 100644 index 0000000000..25e3908ef9 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_pwm.c @@ -0,0 +1,106 @@ +/************************************************************************************ + * + * Copyright (C) 2013, 2015, 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * Jordan MacIntyre + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include + +#include "chip.h" +#include "up_arch.h" +#include "kinetis_pwm.h" + +#ifdef CONFIG_PWM + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_pwm_setup + * + * Description: + * All Kinetis K architectures must provide the following interface to work with + * examples/pwm. + * + ************************************************************************************/ + +int board_pwm_setup(void) +{ + FAR struct pwm_lowerhalf_s *pwm; + static bool initialized = false; + int ret; + + /* Have we already initialized? */ + + if (!initialized) { + /* Call nxphlite_pwminitialize() to get an instance of the PWM interface */ + + pwm = kinetis_pwminitialize(0); + + if (!pwm) { + aerr("ERROR: Failed to get the kinetis PWM lower half\n"); + return -ENODEV; + } + + /* Register the PWM driver at "/dev/pwm0" */ + + ret = pwm_register("/dev/pwm0", pwm); + + if (ret < 0) { + aerr("ERROR: pwm_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif /* CONFIG_PWM */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_sdhc.c b/src/drivers/boards/nxphlite-v1/nxphlite_sdhc.c new file mode 100644 index 0000000000..fbaab03d44 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_sdhc.c @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ****************************************************************************/ +// TODO:Not used on V1 HW +/* A micro Secure Digital (SD) card slot is available on the FRDM-K64F connected to + * the SD Host Controller (SDHC) signals of the MCU. This slot will accept micro + * format SD memory cards. The SD card detect pin (PTE6) is an open switch that + * shorts with VDD when card is inserted. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal K64F Pin + * ------------ ------------- -------- + * DAT0 SDHC0_D0 PTE0 + * DAT1 SDHC0_D1 PTE1 + * DAT2 SDHC0_D2 PTE5 + * CD/DAT3 SDHC0_D3 PTE4 + * CMD SDHC0_CMD PTE3 + * CLK SDHC0_DCLK PTE2 + * SWITCH D_CARD_DETECT PTE6 + * ------------ ------------- -------- + * + * There is no Write Protect pin available to the K64F. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "kinetis.h" + +#include "board_config.h" + +#ifdef HAVE_MMCSD + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ +/* This structure holds static information unique to one SDHC peripheral */ + +struct nxphlite_sdhc_state_s { + struct sdio_dev_s *sdhc; /* R/W device handle */ + bool inserted; /* TRUE: card is inserted */ +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* HSCMI device state */ + +static struct nxphlite_sdhc_state_s g_sdhc; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxphlite_mediachange + ****************************************************************************/ + +static void nxphlite_mediachange(void) +{ + bool inserted; + + /* Get the current value of the card detect pin. This pin is pulled up on + * board. So low means that a card is present. + */ + + inserted = !kinetis_gpioread(GPIO_SD_CARDDETECT); + mcinfo("inserted: %s\n", inserted ? "Yes" : "No"); + + /* Has the pin changed state? */ + + if (inserted != g_sdhc.inserted) { + mcinfo("Media change: %d->%d\n", g_sdhc.inserted, inserted); + + /* Yes.. perform the appropriate action (this might need some debounce). */ + + g_sdhc.inserted = inserted; + sdhc_mediachange(g_sdhc.sdhc, inserted); + +#ifdef CONFIG_NXPHLITE_SDHC_AUTOMOUNT + /* Let the automounter know about the insertion event */ + + nxphlite_automount_event(nxphlite_cardinserted()); +#endif + } +} + +/**************************************************************************** + * Name: nxphlite_cdinterrupt + ****************************************************************************/ + +static int nxphlite_cdinterrupt(int irq, FAR void *context) +{ + /* All of the work is done by nxphlite_mediachange() */ + + nxphlite_mediachange(); + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxphlite_sdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int nxphlite_sdhc_initialize(void) +{ + int ret; + + /* Configure GPIO pins */ + + kinetis_pinconfig(GPIO_SD_CARDDETECT); + + /* Attached the card detect interrupt (but don't enable it yet) */ + + kinetis_pinirqattach(GPIO_SD_CARDDETECT, nxphlite_cdinterrupt); + + /* Configure the write protect GPIO -- None */ + + /* Mount the SDHC-based MMC/SD block driver */ + /* First, get an instance of the SDHC interface */ + + mcinfo("Initializing SDHC slot %d\n", MMCSD_SLOTNO); + + g_sdhc.sdhc = sdhc_initialize(MMCSD_SLOTNO); + + if (!g_sdhc.sdhc) { + mcerr("ERROR: Failed to initialize SDHC slot %d\n", MMCSD_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDHC interface to the MMC/SD driver */ + + mcinfo("Bind SDHC to the MMC/SD driver, minor=%d\n", MMSCD_MINOR); + + ret = mmcsd_slotinitialize(MMSCD_MINOR, g_sdhc.sdhc); + + if (ret != OK) { + syslog(LOG_ERR, "ERROR: Failed to bind SDHC to the MMC/SD driver: %d\n", ret); + return ret; + } + + syslog(LOG_INFO, "Successfully bound SDHC to the MMC/SD driver\n"); + + /* Handle the initial card state */ + + nxphlite_mediachange(); + + /* Enable CD interrupts to handle subsequent media changes */ + + kinetis_pinirqenable(GPIO_SD_CARDDETECT); + return OK; +} + +/**************************************************************************** + * Name: nxphlite_cardinserted + * + * Description: + * Check if a card is inserted into the SDHC slot + * + ****************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +bool nxphlite_cardinserted(void) +{ + bool inserted; + + /* Get the current value of the card detect pin. This pin is pulled up on + * board. So low means that a card is present. + */ + + inserted = !kinetis_gpioread(GPIO_SD_CARDDETECT); + mcinfo("inserted: %s\n", inserted ? "Yes" : "No"); + return inserted; +} +#endif + +/**************************************************************************** + * Name: nxphlite_writeprotected + * + * Description: + * Check if a card is inserted into the SDHC slot + * + ****************************************************************************/ + +#ifdef HAVE_AUTOMOUNTER +bool nxphlite_writeprotected(void) +{ + /* There are no write protect pins */ + + return false; +} +#endif + +#endif /* HAVE_MMCSD */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_sdio.c b/src/drivers/boards/nxphlite-v1/nxphlite_sdio.c new file mode 100644 index 0000000000..c223761812 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_sdio.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file nxphlite-v1_sdio.c + * + * Board-specific SDIO functions. + * V1 of the hardware is ussing SPI for SD card access + * + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "kinetis.h" +#include "board_config.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) syslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message syslog +# else +# define message printf +# endif +#endif + +/* Configuration ************************************************************/ + +/************************************************************************************ + * Private Data + ************************************************************************************/ +static struct spi_dev_s *spi; + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_sdio_initialize + * + * Description: + * Called to configure SDIO. + * + ************************************************************************************/ + +__EXPORT int nxphlite_sdio_initialize(void) +{ + /* Get the SPI port for the microSD slot */ + + spi = kinetis_spibus_initialize(PX4_SPI_BUS_SDCARD); + + if (!spi) { + message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SDCARD); + return -ENODEV; + } + + /* Now bind the SPI interface to the MMCSD driver */ + int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi); + + if (result != OK) { + message("[boot] FAILED to bind SPI port %d to the MMCSD driver\n", PX4_SPI_BUS_SDCARD); + return -ENODEV; + } + + return OK; +} diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_spi.c b/src/drivers/boards/nxphlite-v1/nxphlite_spi.c new file mode 100644 index 0000000000..60e0cca1a4 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_spi.c @@ -0,0 +1,286 @@ +/************************************************************************************ + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +//#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include +#include "board_config.h" +#include +#include + +#if defined(CONFIG_KINETIS_SPI0) || defined(CONFIG_KINETIS_SPI1) || \ + defined(CONFIG_KINETIS_SPI2) + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) syslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message syslog +# else +# define message printf +# endif +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms) +{ + +} + +/************************************************************************************ + * Name: nxphlite_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXPhlite-v1 board. + * + ************************************************************************************/ + +void nxphlite_spidev_initialize(void) +{ + kinetis_pinconfig(GPIO_SPI_CS_SDCARD); + kinetis_pinconfig(GPIO_SPI_CS_FXOS8700CQ_ACCEL_MAG); + kinetis_pinconfig(GPIO_SPI_CS_FXAS21002CQ_GYRO); + + kinetis_pinconfig(GPIO_GM_RST); + kinetis_pinconfig(GPIO_A_RST); + + kinetis_pinconfig(GPIO_EXTI_GYRO_INT1); + kinetis_pinconfig(GPIO_EXTI_GYRO_INT2); + kinetis_pinconfig(GPIO_EXTI_ACCEL_MAG_INT1); + kinetis_pinconfig(GPIO_EXTI_ACCEL_MAG_INT2); + kinetis_pinconfig(GPIO_EXTI_BARO_INT1); + kinetis_pinconfig(GPIO_EXTI_BARO_INT2); +} + +/************************************************************************************ + * Name: kinetis_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXPHLITEV1 board. + * + ************************************************************************************/ +static struct spi_dev_s *spi_accel_mag; +static struct spi_dev_s *spi_baro; + +__EXPORT int nxphlite_spi_bus_initialize(void) +{ + /* Configure SPI-based devices */ + + spi_accel_mag = kinetis_spibus_initialize(PX4_SPI_BUS_ACCEL_MAG); + + if (!spi_accel_mag) { + message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_ACCEL_MAG); + return -ENODEV; + } + + /* Default PX4_SPI_BUS_ACCEL_MAG to 1MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_accel_mag, 1 * 1000 * 1000); + SPI_SETBITS(spi_accel_mag, 8); + SPI_SETMODE(spi_accel_mag, SPIDEV_MODE3); + + for (int cs = PX4_ACCEL_MAG_BUS_FIRST_CS; cs <= PX4_ACCEL_MAG_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_accel_mag, cs, false); + } + + /* Get the SPI port for the GYRO */ + + spi_baro = kinetis_spibus_initialize(PX4_SPI_BUS_GYRO); + + if (!spi_baro) { + message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_GYRO); + return -ENODEV; + } + + /* FXAS21002CQ has max SPI clock speed of 2MHz and uses MODE 0 (CPOL = 0, and CPHA = 0) + */ + + SPI_SETFREQUENCY(spi_baro, 2 * 1000 * 1000); + SPI_SETBITS(spi_baro, 8); + SPI_SETMODE(spi_baro, SPIDEV_MODE0); + + for (int cs = PX4_GYRO_BUS_FIRST_CS; cs <= PX4_GYRO_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_baro, cs, false); + } + + return OK; + +} + +/************************************************************************************ + * Name: kinetis_spi[n]select, kinetis_spi[n]status, and kinetis_spi[n]cmddata + * + * Description: + * These external functions must be provided by board-specific logic. They are + * implementations of the select, status, and cmddata methods of the SPI interface + * defined by struct spi_ops_s (see include/nuttx/spi/spi.h). All other methods + * including kinetis_spibus_initialize()) are provided by common Kinetis logic. + * To use this common SPI logic on your board: + * + * 1. Provide logic in kinetis_boardinitialize() to configure SPI chip select + * pins. + * 2. Provide kinetis_spi[n]select() and kinetis_spi[n]status() functions + * in your board-specific logic. These functions will perform chip selection + * and status operations using GPIOs in the way your board is configured. + * 2. If CONFIG_SPI_CMDDATA is defined in the NuttX configuration, provide + * kinetis_spi[n]cmddata() functions in your board-specific logic. These + * functions will perform cmd/data selection operations using GPIOs in the way + * your board is configured. + * 3. Add a call to kinetis_spibus_initialize() in your low level application + * initialization logic + * 4. The handle returned by kinetis_spibus_initialize() may then be used to bind the + * SPI driver to higher level logic (e.g., calling + * mmcsd_spislotinitialize(), for example, will bind the SPI driver to + * the SPI MMC/SD driver). + * + ************************************************************************************/ + +static const uint32_t spi0selects_gpio[] = PX4_SDCARD_BUS_CS_GPIO; + +void kinetis_spi0select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); + + /* SPI select is active low, so write !selected to select the device */ + + int sel = (int) devid; + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_SDCARD); + + /* Making sure the other peripherals are not selected */ + + for (int cs = 0; arraySize(spi0selects_gpio) > 1 && cs < arraySize(spi0selects_gpio); cs++) { + if (spi0selects_gpio[cs] != 0) { + kinetis_gpiowrite(spi0selects_gpio[cs], 1); + } + } + + uint32_t gpio = spi0selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + kinetis_gpiowrite(gpio, !selected); + } +} + +uint8_t kinetis_spi0status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +static const uint32_t spi1selects_gpio[] = PX4_ACCEL_MAG_BUS_CS_GPIO; + +void kinetis_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); + + /* SPI select is active low, so write !selected to select the device */ + + int sel = (int) devid; + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_ACCEL_MAG); + + /* Making sure the other peripherals are not selected */ + + for (int cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) { + if (spi1selects_gpio[cs] != 0) { + kinetis_gpiowrite(spi1selects_gpio[cs], 1); + } + } + + uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + kinetis_gpiowrite(gpio, !selected); + } +} + +uint8_t kinetis_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +static const uint32_t spi2selects_gpio[] = PX4_GYRO_BUS_CS_GPIO; + +void kinetis_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); + + /* SPI select is active low, so write !selected to select the device */ + + int sel = (int) devid; + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_GYRO); + + /* Making sure the other peripherals are not selected */ + + for (int cs = 0; arraySize(spi2selects_gpio) > 1 && cs < arraySize(spi2selects_gpio); cs++) { + if (spi2selects_gpio[cs] != 0) { + kinetis_gpiowrite(spi2selects_gpio[cs], 1); + } + } + + uint32_t gpio = spi2selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + kinetis_gpiowrite(gpio, !selected); + } +} + +uint8_t kinetis_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#endif /* CONFIG_KINETIS_SPI0 || CONFIG_KINETIS_SPI1 || CONFIG_KINETIS_SPI2 */ diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_timer_config.c b/src/drivers/boards/nxphlite-v1/nxphlite_timer_config.c new file mode 100644 index 0000000000..7a4c2eb773 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_timer_config.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file nxphlite_timer_config.c + * + * Configuration data for the kinetis pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +// TODO:Stubbed out for now +#include + +#include +//#include +//#include // TODO:Stubbed in for now +//#include +//#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { +}; diff --git a/src/drivers/boards/nxphlite-v1/nxphlite_usb.c b/src/drivers/boards/nxphlite-v1/nxphlite_usb.c new file mode 100644 index 0000000000..bdbf2f85a5 --- /dev/null +++ b/src/drivers/boards/nxphlite-v1/nxphlite_usb.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file nxphlite_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: nxphlite_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the NXPhlite-v1 board. + * + ************************************************************************************/ + +__EXPORT +void nxphlite_usbinitialize(void) +{ +} + +/************************************************************************************ + * Name: kinetis_usbpullup + * + * Description: + * If USB is supported and the board supports a pullup via GPIO (for USB software + * connect and disconnect), then the board software must provide kinetis_usbpullup. + * See include/nuttx/usb/usbdev.h for additional description of this method. + * Alternatively, if no pull-up GPIO the following EXTERN can be redefined to be + * NULL. + * + ************************************************************************************/ + +__EXPORT +int kinetis_usbpullup(FAR struct usbdev_s *dev, bool enable) +{ + usbtrace(TRACE_DEVPULLUP, (uint16_t)enable); + return OK; +} + +/************************************************************************************ + * Name: kinetis_usbsuspend + * + * Description: + * Board logic must provide the kinetis_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 kinetis_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/src/drivers/kinetis/CMakeLists.txt b/src/drivers/kinetis/CMakeLists.txt new file mode 100644 index 0000000000..a28f4d7458 --- /dev/null +++ b/src/drivers/kinetis/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2016 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__kinetis + COMPILE_FLAGS + SRCS + drv_hrt.c + drv_io_timer.c + drv_pwm_servo.c + drv_input_capture.c + drv_led_pwm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/kinetis/adc/CMakeLists.txt b/src/drivers/kinetis/adc/CMakeLists.txt new file mode 100644 index 0000000000..ba29e2eaa2 --- /dev/null +++ b/src/drivers/kinetis/adc/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2016 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__kinetis__adc + MAIN adc + COMPILE_FLAGS + SRCS + adc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/kinetis/adc/adc.cpp b/src/drivers/kinetis/adc/adc.cpp new file mode 100644 index 0000000000..e674030b1c --- /dev/null +++ b/src/drivers/kinetis/adc/adc.cpp @@ -0,0 +1,492 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adc.cpp + * + * TODO:This is stubbed out leving the code intact to document the needed + * mechinsm for porting. + * + * Driver for the Kinetis ADC. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADC driver. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +//#include + +#include +#include + +#include +#include + +#if defined(ADC_CHANNELS) +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +static volatile uint32_t dummy[21]; +#define REG(_reg) (*(volatile uint32_t *)(&dummy[(_reg)])) + +#define ADC_CR2_ADON 0 +#define ADC_CR2_SWSTART 0 +#define ADC_SR_EOC 0 + +#define rSR REG(0) +#define rCR1 REG(1) +#define rCR2 REG(2) +#define rSMPR1 REG(3) +#define rSMPR2 REG(4) +#define rJOFR1 REG(5) +#define rJOFR2 REG(6) +#define rJOFR3 REG(7) +#define rJOFR4 REG(8) +#define rHTR REG(9) +#define rLTR REG(10) +#define rSQR1 REG(11) +#define rSQR2 REG(12) +#define rSQR3 REG(13) +#define rJSQR REG(14) +#define rJDR1 REG(15) +#define rJDR2 REG(16) +#define rJDR3 REG(17) +#define rJDR4 REG(18) +#define rDR REG(19) +# define rCCR REG(20) + +class ADC : public device::CDev +{ +public: + ADC(uint32_t channels); + ~ADC(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t read(file *filp, char *buffer, size_t len); + +protected: + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + +private: + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + + hrt_call _call; + perf_counter_t _sample_perf; + + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + orb_advert_t _to_system_power; + orb_advert_t _to_adc_report; + + /** work trampoline */ + static void _tick_trampoline(void *arg); + + /** worker function */ + void _tick(); + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ + uint16_t _sample(unsigned channel); + + // update system_power ORB topic, only on FMUv2 + void update_system_power(hrt_abstime now); + + void update_adc_report(hrt_abstime now); +}; + +ADC::ADC(uint32_t channels) : + CDev("adc", ADC0_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), + _channel_count(0), + _samples(nullptr), + _to_system_power(nullptr), + _to_adc_report(nullptr) +{ + _debug_enabled = true; + + /* always enable the temperature sensor */ + channels |= 1 << 16; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } +} + +ADC::~ADC() +{ + if (_samples != nullptr) { + delete _samples; + } +} + +int +ADC::init() +{ + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_CAL; + usleep(100); + + if (rCR2 & ADC_CR2_CAL) { + return -1; + } + +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2 |= ADC_CR2_SWSTART; + + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + DEVICE_LOG("sample timeout"); + return -1; + } + } + + + /* create the device node */ + return CDev::init(); +} + +int +ADC::ioctl(file *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADC::read(file *filp, char *buffer, size_t len) +{ + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) { + len = maxsize; + } + + /* block interrupts while copying samples to avoid racing with an update */ + irqstate_t flags = px4_enter_critical_section(); + memcpy(buffer, _samples, len); + px4_leave_critical_section(flags); + + return len; +} + +int +ADC::open_first(struct file *filp) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADC::close_last(struct file *filp) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADC::_tick_trampoline(void *arg) +{ + (reinterpret_cast(arg))->_tick(); +} + +void +ADC::_tick() +{ + hrt_abstime now = hrt_absolute_time(); + + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) { + _samples[i].am_data = _sample(_samples[i].am_channel); + } + + update_adc_report(now); + update_system_power(now); +} + +void +ADC::update_adc_report(hrt_abstime now) +{ + adc_report_s adc = {}; + adc.timestamp = now; + + unsigned max_num = _channel_count; + + if (max_num > (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]))) { + max_num = (sizeof(adc.channel_id) / sizeof(adc.channel_id[0])); + } + + for (unsigned i = 0; i < max_num; i++) { + adc.channel_id[i] = _samples[i].am_channel; + adc.channel_value[i] = _samples[i].am_data * 3.3f / 4096.0f; + } + + int instance; + orb_publish_auto(ORB_ID(adc_report), &_to_adc_report, &adc, &instance, ORB_PRIO_HIGH); +} + +void +ADC::update_system_power(hrt_abstime now) +{ +#if defined (BOARD_ADC_USB_CONNECTED) + system_power_s system_power = {}; + system_power.timestamp = now; + + system_power.voltage5V_v = 0; + +#if defined(ADC_5V_RAIL_SENSE) + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + +#endif + + /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED, + * It must provide the true logic GPIO BOARD_ADC_xxxx macros. + */ + // these are not ADC related, but it is convenient to + // publish these to the same topic + + system_power.usb_connected = BOARD_ADC_USB_CONNECTED; + + system_power.brick_valid = BOARD_ADC_BRICK_VALID; + system_power.servo_valid = BOARD_ADC_SERVO_VALID; + + // OC pins are active low + system_power.periph_5V_OC = BOARD_ADC_PERIPH_5V_OC; + system_power.hipower_5V_OC = BOARD_ADC_HIPOWER_5V_OC; + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // BOARD_ADC_USB_CONNECTED +} + +uint16_t +ADC::_sample(unsigned channel) +{ + perf_begin(_sample_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) { + rSR &= ~ADC_SR_EOC; + } + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + DEVICE_LOG("sample timeout"); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(_sample_perf); + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + + int fd = open(ADC0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "can't open ADC device"); + } + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[12]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) { + errx(1, "read error"); + } + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ + g_adc = new ADC(ADC_CHANNELS); + + if (g_adc == nullptr) { + errx(1, "couldn't allocate the ADC driver"); + } + + if (g_adc->init() != OK) { + delete g_adc; + errx(1, "ADC init failed"); + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) { + test(); + } + } + + exit(0); +} +#endif diff --git a/src/drivers/kinetis/drv_hrt.c b/src/drivers/kinetis/drv_hrt.c new file mode 100644 index 0000000000..fa601527e9 --- /dev/null +++ b/src/drivers/kinetis/drv_hrt.c @@ -0,0 +1,911 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * This can use any general or advanced STM32 timer. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX STM32 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + + +#include "kinetis.h" +//#include "kinetis_pit.h" + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +#ifdef HRT_TIMER + +/* HRT configuration */ +//# define HRT_TIMER_BASE STM32_TIM1_BASE +//# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +//# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define HRT_TIMER_VECTOR 0 //STM32_IRQ_TIM1CC +#define HRT_TIMER_CLOCK 60000000 +//# if CONFIG_STM32_TIM1 +//# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1 +//# endif +//#elif HRT_TIMER == 2 + +/* +* HRT clock must be a multiple of 1MHz greater than 1MHz +*/ +#if (HRT_TIMER_CLOCK % 1000000) != 0 +# error HRT_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if HRT_TIMER_CLOCK <= 1000000 +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/** +* Minimum/maximum deadlines. +* +* These are suitable for use with a 16-bit timer/counter clocked +* at 1MHz. The high-resolution timer need only guarantee that it +* not wrap more than once in the 50ms period for absolute time to +* be consistently maintained. +* +* The minimum deadline must be such that the time taken between +* reading a time and writing a deadline to the timer cannot +* result in missing the deadline. +*/ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* +* Period of the free-running counter, in microseconds. +*/ +#define HRT_COUNTER_PERIOD 65536 + +/* +* Scaling factor(s) for the free-running counter; convert an input +* in counts to a time in microseconds. +*/ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* TODO:This is stubbed out leving the code intact to document the needed +* mechinsm for porting. +*/ + +/* +* Timer register accessors +*/ +static volatile uint32_t dummy[18]; +#define REG(_reg) (*(volatile uint32_t *)(&dummy[(_reg)])) +#undef modifyreg32 +#define modifyreg32(reg,clr,set) +#define HRT_TIMER_VECTOR 0 +#define irq_attach(HRT_TIMER_VECTOR, isr) (void)(isr) + +#define rCR1 REG(0) +#define rCR2 REG(1) +#define rSMCR REG(2) +#define rDIER REG(3) +#define rSR REG(4) +#define rEGR REG(5) +#define rCCMR1 REG(6) +#define rCCMR2 REG(7) +#define rCCER REG(8) +#define rCNT REG(9) +#define rPSC REG(10) +#define rARR REG(11) +#define rCCR1 REG(12) +#define rCCR2 REG(13) +#define rCCR3 REG(14) +#define rCCR4 REG(15) +#define rDCR REG(16) +#define rDMAR REG(17) + + +#define GTIM_EGR_UG 0 +#define GTIM_CR1_CEN 0 + + +/* +* Specific registers and bits used by HRT sub-functions +*/ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ +#if HRT_TIMER_CHANNEL == 1 +# define rCCR_HRT rCCR1 /* compare register for HRT */ +# define DIER_HRT 0 //GTIM_DIER_CC1IE /* interrupt enable for HRT */ +# define SR_INT_HRT 1 //GTIM_SR_CC1IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 2 +# define rCCR_HRT rCCR2 /* compare register for HRT */ +# define DIER_HRT 2//GTIM_DIER_CC2IE /* interrupt enable for HRT */ +# define SR_INT_HRT 3//GTIM_SR_CC2IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 3 +# define rCCR_HRT rCCR3 /* compare register for HRT */ +# define DIER_HRT 4//GTIM_DIER_CC3IE /* interrupt enable for HRT */ +# define SR_INT_HRT 7// GTIM_SR_CC3IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 4 +# define rCCR_HRT rCCR4 /* compare register for HRT */ +# define DIER_HRT 5//GTIM_DIER_CC4IE /* interrupt enable for HRT */ +# define SR_INT_HRT 6//GTIM_SR_CC4IF /* interrupt status for HRT */ +#else +# error HRT_TIMER_CHANNEL must be a value between 1 and 4 +#endif + +/* +* Queue of callout entries. +*/ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/* +* Specific registers and bits used by PPM sub-functions +*/ +#ifdef HRT_PPM_CHANNEL +/* +* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. +*/ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ +//# if HRT_PPM_CHANNEL == 1 +# define rCCR_PPM rCCR1 /* capture register for PPM */ +# define DIER_PPM 0//GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM 1//GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM 2//GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 1 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM 8//(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP 2//GTIM_CCER_CC1P +//# elif HRT_PPM_CHANNEL == 2 +//# else +//# error HRT_PPM_CHANNEL must be a value between 1 and 4 + +/* +* PPM decoder tuning parameters +*/ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 5 +#define PPM_MAX_CHANNELS 20 + +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ + +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; +__EXPORT unsigned ppm_decoded_channels = 0; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +#define PPM_DEBUG 0 + +#if PPM_DEBUG +/* PPM edge history */ +__EXPORT uint16_t ppm_edge_history[32]; +unsigned ppm_edge_next; + +/* PPM pulse history */ +__EXPORT uint16_t ppm_pulse_history[32]; +unsigned ppm_pulse_next; +#endif + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/** PPM decoder state machine */ +struct { + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +static void hrt_ppm_decode(uint32_t status); + +#else +/* disable the PPM configuration */ +# define rCCR_PPM 0 +# define DIER_PPM 0 +# define SR_INT_PPM 0 +# define SR_OVF_PPM 0 +# define CCMR1_PPM 0 +# define CCMR2_PPM 0 +# define CCER_PPM 0 +#endif /* HRT_PPM_CHANNEL */ + +/** +* Initialise the timer we are going to use. +* +* We expect that we'll own one of the reduced-function STM32 general +* timers, and that we can use channel 1 in compare mode. +*/ +static void +hrt_tim_init(void) +{ + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + + /* clock/power on our timer */ + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + + /* run the full span of the counter */ + rARR = 0xffff; + + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +#ifdef HRT_PPM_CHANNEL +/** +* Handle the PPM decoder state machine. +*/ +static void +hrt_ppm_decode(uint32_t status) +{ + uint16_t count = rCCR_PPM; + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PPM) { + goto error; + } + + /* how long since the last edge? - this handles counter wrapping implicitely. */ + width = count - ppm.last_edge; + +#if PPM_DEBUG + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) { + ppm_edge_next = 0; + } + +#endif + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) { + ppm_buffer[i] = ppm_temp_buffer[i]; + } + + ppm_last_valid_decode = hrt_absolute_time(); + + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + ppm.last_edge = count; + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + break; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; + + case INACTIVE: + + /* we expect a short pulse */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + break; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + +#if PPM_DEBUG + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) { + ppm_pulse_next = 0; + } + +#endif + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { + goto error; + } + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) { + ppm_temp_buffer[ppm.next_channel++] = interval; + } + + ppm.phase = INACTIVE; + break; + + } + + ppm.last_edge = count; + return; + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + +} +#endif /* HRT_PPM_CHANNEL */ + +/** +* Handle the compare interrupt by calling the callout dispatcher +* and then re-scheduling the next deadline. +*/ +static int +hrt_tim_isr(int irq, void *context) +{ + uint32_t status; + + /* grab the timer for latency tracking purposes */ + latency_actual = rCNT; + + /* copy interrupt status */ + status = rSR; + + /* ack the interrupts we just read */ + rSR = ~status; + +#ifdef HRT_PPM_CHANNEL + + /* was this a PPM edge? */ + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + + hrt_ppm_decode(status); + } + +#endif + + /* was this a timer tick? */ + if (status & SR_INT_HRT) { + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + } + + return OK; +} + +/** +* Fetch a never-wrapping absolute time value in microseconds from +* some arbitrary epoch shortly after system start. +*/ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime; + uint32_t count; + irqstate_t flags; + + /* + * Counter state. Marked volatile as they may change + * inside this routine but outside the irqsave/restore + * pair. Discourage the compiler from moving loads/stores + * to these outside of the protected range. + */ + static volatile hrt_abstime base_time; + static volatile uint32_t last_count; + + /* prevent re-entry */ + flags = px4_enter_critical_section(); + + /* get the current counter value */ + count = rCNT; + + /* + * Determine whether the counter has wrapped since the + * last time we're called. + * + * This simple test is sufficient due to the guarantee that + * we are always called at least once per counter period. + */ + if (count < last_count) { + base_time += HRT_COUNTER_PERIOD; + } + + /* save the count for next time */ + last_count = count; + + /* compute the current time */ + abstime = HRT_COUNTER_SCALE(base_time + count); + + px4_leave_critical_section(flags); + + return abstime; +} + +/** +* Convert a timespec to absolute time +*/ +hrt_abstime +ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/** +* Convert absolute time to a timespec. +*/ +void +abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +/** +* Compare a time value with the current time. +*/ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = px4_enter_critical_section(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + px4_leave_critical_section(flags); + + return delta; +} + +/** +* Store the absolute time in an interrupt-safe fashion +*/ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = px4_enter_critical_section(); + + hrt_abstime ts = hrt_absolute_time(); + + px4_leave_critical_section(flags); + + return ts; +} + +/** +* Initialise the high-resolution timing module. +*/ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef HRT_PPM_CHANNEL + /* configure the PPM input pin */ + px4_arch_configgpio(GPIO_PPM_IN); +#endif +} + +/** +* Call callout(arg) after interval has elapsed. +*/ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/** +* Call callout(arg) at calltime. +*/ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/** +* Call callout(arg) every period. +*/ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + px4_leave_critical_section(flags); +} + +/** +* If this returns true, the call has been invoked and removed from the callout list. +* +* Always returns false for repeating callouts. +*/ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/** +* Remove the entry from the callout list. +*/ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = px4_enter_critical_section(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + px4_leave_critical_section(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrtinfo("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + hrtinfo("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + hrtinfo("call % p: % p( % p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** +* Reschedule the next timer interrupt. +* +* This routine must be called with interrupts disabled. +*/ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre - expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + hrtinfo("due soon\n"); + deadline = next->deadline; + } + } + + hrtinfo("schedule for % u at % u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + rCCR_HRT = latency_baseline = deadline & 0xffff; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +#endif /* HRT_TIMER */ diff --git a/src/drivers/kinetis/drv_input_capture.c b/src/drivers/kinetis/drv_input_capture.c new file mode 100644 index 0000000000..4a229fa238 --- /dev/null +++ b/src/drivers/kinetis/drv_input_capture.c @@ -0,0 +1,517 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_input_capture.c + * + * Servo driver supporting input capture connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have input pins. + * + * Require an interrupt. + * + * The use of thie interface is mutually exclusive with the pwm + * because the same timers are used and there is a resource contention + * with the ARR as it sets the pwm rate and in this driver needs to match + * that of the hrt to back calculate the actual point in time the edge + * was detected. + * + * This is accomplished by taking the difference between the current + * count rCNT snapped at the time interrupt and the rCCRx captured on the + * edge transition. This delta is applied to hrt time and the resulting + * value is the absolute system time the edge occured. + * + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "drv_io_timer.h" + +#include "drv_input_capture.h" + +#include +//#include +#define GTIM_CCMR1_IC1F_MASK 0 +static volatile uint32_t dummy[4]; +#define __REG32(_reg) (*(volatile uint32_t *)(&dummy[(_reg)])) +#define STM32_GTIM_SR_OFFSET 0 +#define GTIM_CCMR1_IC1F_SHIFT 0 +#define GTIM_CCMR1_IC2F_MASK 0 +#define GTIM_CCMR1_IC2F_SHIFT 0 +#define GTIM_CCMR1_IC3F_SHIFT 0 +#define GTIM_CCMR1_IC3F_MASK 0 +#define GTIM_CCMR2_IC3F_MASK 0 +#define GTIM_CCMR2_IC3F_SHIFT 0 +#define GTIM_CCMR2_IC4F_MASK 0 +#define GTIM_CCMR2_IC4F_SHIFT 0 +#define GTIM_CCER_CC1P 12 +#define GTIM_CCER_CC1NP 14 + +#define _REG32(_base, _reg) __REG32(_reg) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + +#define rCCMR1(_tmr) REG(_tmr, 0) +#define rCCMR2(_tmr) REG(_tmr, 1) +#define rCCER(_tmr) REG(_tmr, 2) + +#define GTIM_SR_CCOF 0 //(GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF) + +static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + capture_callback_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + + +static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt) +{ + uint16_t capture = _REG32(timer, chan->ccr_offset); + channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); + + if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { + channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + } + + channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); + uint32_t overflow = _REG32(timer, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF; + + if (overflow) { + + /* Error we has a second edge before we cleared CCxR */ + + channel_stats[chan_index].overflows++; + } + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index, + channel_stats[chan_index].last_time, + channel_stats[chan_index].last_edge, overflow); + } +} + +static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context) +{ + irqstate_t flags = px4_enter_critical_section(); + channel_handlers[channel].callback = callback; + channel_handlers[channel].context = context; + px4_leave_critical_section(flags); +} + +static void input_capture_unbind(unsigned channel) +{ + input_capture_bind(channel, NULL, NULL); +} + +int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, + capture_callback_t callback, void *context) +{ + if (filter > GTIM_CCMR1_IC1F_MASK) { + return -EINVAL; + } + + if (edge > Both) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (edge == Disabled) { + + io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); + input_capture_unbind(channel); + + } else { + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + input_capture_bind(channel, callback, context); + + rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); + + if (rv != 0) { + return rv; + } + + rv = up_input_capture_set_filter(channel, filter); + + if (rv == 0) { + rv = up_input_capture_set_trigger(channel, edge); + + if (rv == 0) { + rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel); + } + } + } + } + + return rv; +} + +int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + uint32_t timer = timer_io_channels[channel].timer_index; + (void) timer; + uint16_t rvalue; + rv = OK; + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rCCMR1(timer) & GTIM_CCMR1_IC1F_MASK; + *filter = (rvalue << GTIM_CCMR1_IC1F_SHIFT); + break; + + case 2: + rvalue = rCCMR1(timer) & GTIM_CCMR1_IC2F_MASK; + *filter = (rvalue << GTIM_CCMR1_IC2F_SHIFT); + break; + + case 3: + rvalue = rCCMR2(timer) & GTIM_CCMR2_IC3F_MASK; + *filter = (rvalue << GTIM_CCMR2_IC3F_SHIFT); + break; + + case 4: + rvalue = rCCMR2(timer) & GTIM_CCMR2_IC4F_MASK; + *filter = (rvalue << GTIM_CCMR2_IC4F_SHIFT); + break; + + default: + rv = -EIO; + } + } + } + + return rv; +} +int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) +{ + if (filter > GTIM_CCMR1_IC1F_MASK) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; +// uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + + irqstate_t flags = px4_enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC1F_MASK; + rvalue |= (filter << GTIM_CCMR1_IC1F_SHIFT); + rCCMR1(timer) = rvalue; + break; + + case 2: + rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC2F_MASK; + rvalue |= (filter << GTIM_CCMR1_IC2F_SHIFT); + rCCMR1(timer) = rvalue; + break; + + case 3: + rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC3F_MASK; + rvalue |= (filter << GTIM_CCMR2_IC3F_SHIFT); + rCCMR2(timer) = rvalue; + break; + + case 4: + rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC4F_MASK; + rvalue |= (filter << GTIM_CCMR2_IC4F_SHIFT); + rCCMR1(timer) = rvalue; + break; + + default: + rv = -EIO; + } + + px4_leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + +// uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rCCER(timer) ;//& (GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + break; + + case 2: + rvalue = rCCER(timer) ;//& (GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rvalue >>= 4; + break; + + case 3: + rvalue = rCCER(timer) ;//& (GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rvalue >>= 8; + break; + + case 4: + rvalue = rCCER(timer) ;//& (GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rvalue >>= 12; + break; + + default: + rv = -EIO; + } + + if (rv == 0) { + switch (rvalue) { + + case 0: + *edge = Rising; + break; + + case (GTIM_CCER_CC1P | GTIM_CCER_CC1NP): + *edge = Both; + break; + + case (GTIM_CCER_CC1P): + *edge = Falling; + break; + + default: + rv = -EIO; + } + } + } + } + + return rv; +} +int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + uint16_t edge_bits = 0xffff; + + switch (edge) { + case Disabled: + break; + + case Rising: + edge_bits = 0; + break; + + case Falling: + edge_bits = GTIM_CCER_CC1P; + break; + + case Both: + edge_bits = GTIM_CCER_CC1P | GTIM_CCER_CC1NP; + break; + + default: + return -EINVAL;; + } + + //uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + rv = OK; + + irqstate_t flags = px4_enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rCCER(timer); + rvalue &= ~(GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rvalue |= edge_bits; + rCCER(timer) = rvalue; + break; + + case 2: + rvalue = rCCER(timer); +// rvalue &= ~(GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rvalue |= (edge_bits << 4); + rCCER(timer) = rvalue; + break; + + case 3: + rvalue = rCCER(timer); + //rvalue &= ~(GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rvalue |= edge_bits << 8; + rCCER(timer) = rvalue; + break; + + case 4: + rvalue = rCCER(timer); +// rvalue &= ~(GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rvalue |= edge_bits << 12; + rCCER(timer) = rvalue; + break; + + default: + rv = -EIO; + } + + px4_leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + irqstate_t flags = px4_enter_critical_section(); + *callback = channel_handlers[channel].callback; + *context = channel_handlers[channel].context; + px4_leave_critical_section(flags); + rv = OK; + } + } + + return rv; + +} + +int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + input_capture_bind(channel, callback, context); + rv = 0; + } + } + + return rv; +} + +int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + irqstate_t flags = px4_enter_critical_section(); + *stats = channel_stats[channel]; + + if (clear) { + memset(&channel_stats[channel], 0, sizeof(*stats)); + } + + px4_leave_critical_section(flags); + } + + return rv; +} diff --git a/src/drivers/kinetis/drv_input_capture.h b/src/drivers/kinetis/drv_input_capture.h new file mode 100644 index 0000000000..968ce831ef --- /dev/null +++ b/src/drivers/kinetis/drv_input_capture.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_input_capture.h + * + * stm32-specific input capture data. + */ + +#pragma once + +#include diff --git a/src/drivers/kinetis/drv_io_timer.c b/src/drivers/kinetis/drv_io_timer.c new file mode 100644 index 0000000000..f3368c36e1 --- /dev/null +++ b/src/drivers/kinetis/drv_io_timer.c @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drv_io_timer.h" + +#include +//#include + +#define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) + +#define MAX_CHANNELS_PER_TIMER 4 +#define STM32_TIM8_BASE 0 +#define STM32_TIM1_BASE 0 +#define ATIM_BDTR_MOE 0 +#define CCMR_C1_CAPTURE_INIT 0 +#define GTIM_CCER_CC1E 0 +#define GTIM_DIER_CC1IE 0 +#define CCMR_C1_PWMOUT_INIT 0 +#define STM32_GTIM_CCMR1_OFFSET 0 +#define STM32_GTIM_CCR1_OFFSET 0 +#define STM32_GTIM_CCER_OFFSET +#define modifyreg32(r,c,s) + +#define GTIM_SR_UIF (1 << 0) /* Bit 0: Update interrupt flag */ +#define GTIM_SR_CC1IF (1 << 1) /* Bit 1: Capture/compare 1 interrupt flag */ +#define GTIM_SR_CC2IF (1 << 2) /* Bit 2: Capture/Compare 2 interrupt flag (TIM2-5,9,12,&15 only) */ +#define GTIM_SR_CC3IF (1 << 3) /* Bit 3: Capture/Compare 3 interrupt flag (TIM2-5 only) */ +#define GTIM_SR_CC4IF (1 << 4) /* Bit 4: Capture/Compare 4 interrupt flag (TIM2-5 only) */ +#define GTIM_SR_COMIF (1 << 5) /* Bit 5: COM interrupt flag (TIM15-17 only) */ +#define GTIM_SR_TIF (1 << 6) /* Bit 6: Trigger interrupt Flag (TIM2-5,9,12&15-17 only) */ +#define GTIM_SR_BIF (1 << 7) /* Bit 7: Break interrupt flag (TIM15-17 only) */ +#define GTIM_SR_CC1OF (1 << 9) /* Bit 9: Capture/Compare 1 Overcapture flag */ +#define GTIM_SR_CC2OF (1 << 10) /* Bit 10: Capture/Compare 2 Overcapture flag (TIM2-5,9,12&15 only) */ +#define GTIM_SR_CC3OF (1 << 11) /* Bit 11: Capture/Compare 3 Overcapture flag (TIM2-5 only) */ +#define GTIM_SR_CC4OF (1 << 12) /* Bit 12: Capture/Compare 4 Overcapture flag (TIM2-5 only) */ +#define GTIM_EGR_UG 0 +#define STM32_GTIM_DIER_OFFSET 0 +static volatile uint32_t dummy[19]; +#define __REG32(_reg) (*(volatile uint32_t *)(&dummy[(_reg)])) + +#define _REG32(_base, _reg) __REG32(_reg) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + +#define rCR1(_tmr) REG(_tmr, 0) +#define rCR2(_tmr) REG(_tmr, 1) +#define rSMCR(_tmr) REG(_tmr, 2) +#define rDIER(_tmr) REG(_tmr, 3) +#define rSR(_tmr) REG(_tmr, 4) +#define rEGR(_tmr) REG(_tmr, 5) +#define rCCMR1(_tmr) REG(_tmr, 6) +#define rCCMR2(_tmr) REG(_tmr, 7) +#define rCCER(_tmr) REG(_tmr, 8) +#define rCNT(_tmr) REG(_tmr, 9) +#define rPSC(_tmr) REG(_tmr, 10) +#define rARR(_tmr) REG(_tmr, 11) +#define rCCR1(_tmr) REG(_tmr, 12) +#define rCCR2(_tmr) REG(_tmr, 13) +#define rCCR3(_tmr) REG(_tmr, 14) +#define rCCR4(_tmr) REG(_tmr, 15) +#define rDCR(_tmr) REG(_tmr, 16) +#define rDMAR(_tmr) REG(_tmr, 17) +#define rBDTR(_tmr) REG(_tmr, 18) + +#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF) +#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF) + +#define CCMR_C1_RESET 0x00ff +#define CCMR_C1_NUM_BITS 8 +#define CCER_C1_NUM_BITS 4 + +#define CCMR_C1_CAPTURE_INIT 0 +//(GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | +// (GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | +// (GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT) + +#define CCMR_C1_PWMOUT_INIT 0 //(GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE + +#define CCMR_C1_PWMIN_INIT 0 // TBD + +// NotUsed PWMOut PWMIn Capture +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0 }; + +typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ + +static io_timer_allocation_t once = 0; + +typedef struct channel_stat_t { + uint32_t isr_cout; + uint32_t overflows; +} channel_stat_t; + +static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + channel_handler_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + +static int io_timer_handler(uint16_t timer_index) +{ + /* Read the count at the time of the interrupt */ + + uint16_t count = rCNT(timer_index); + + /* Read the HRT at the time of the interrupt */ + + hrt_abstime now = hrt_absolute_time(); + + const io_timers_t *tmr = &io_timers[timer_index]; + + /* What is pending and enabled? */ + + uint16_t statusr = rSR(timer_index); + uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF; + + /* Iterate over the timer_io_channels table */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + + uint16_t masks = timer_io_channels[chan_index].masks; + + /* Do we have an enabled channel */ + + if (enabled & masks) { + + + if (statusr & masks & GTIM_SR_CCIF) { + + io_timer_channel_stats[chan_index].isr_cout++; + + /* Call the client to read the CCxR etc and clear the CCxIF */ + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, + chan_index, &timer_io_channels[chan_index], + now, count); + } + } + + if (statusr & masks & GTIM_SR_CCOF) { + + /* Error we has a second edge before we cleared CCxR */ + + io_timer_channel_stats[chan_index].overflows++; + } + } + } + + /* Clear all the SR bits for interrupt enabled channels only */ + + rSR(timer_index) = ~(statusr & (enabled | enabled << 8)); + return 0; +} + +int io_timer_handler0(int irq, void *context) +{ + + return io_timer_handler(0); +} + +int io_timer_handler1(int irq, void *context) +{ + return io_timer_handler(1); + +} + +int io_timer_handler2(int irq, void *context) +{ + return io_timer_handler(2); + +} + +int io_timer_handler3(int irq, void *context) +{ + return io_timer_handler(3); + +} + +static inline int validate_timer_index(unsigned timer) +{ + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +static inline int is_timer_uninitalized(unsigned timer) +{ + int rv = 0; + + if (once & 1 << timer) { + rv = -EBUSY; + } + + return rv; +} + +static inline void set_timer_initalized(unsigned timer) +{ + once |= 1 << timer; +} + +static inline void set_timer_deinitalized(unsigned timer) +{ + once &= ~(1 << timer); +} + +static inline int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + + if (validate_timer_index(timer) == 0) { + const io_timers_t *tmr = &io_timers[timer]; + /* Gather the channel bit that belong to the timer */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + } + + return channels; +} + +static inline int is_channels_timer_uninitalized(unsigned channel) +{ + return is_timer_uninitalized(channels_timer(channel)); +} + +int io_timer_is_channel_free(unsigned channel) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { + rv = -EBUSY; + } + } + + return rv; +} + +int io_timer_validate_channel_index(unsigned channel) +{ + int rv = -EINVAL; + + if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { + + unsigned timer = timer_io_channels[channel].timer_index; + + /* test timer for validity */ + + if ((io_timers[timer].base != 0) && + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { + rv = 0; + } + } + + return rv; +} + +int io_timer_get_mode_channels(io_timer_channel_mode_t mode) +{ + if (mode < IOTimerChanModeSize) { + return channel_allocations[mode]; + } + + return 0; +} + +int io_timer_get_channel_mode(unsigned channel) +{ + io_timer_channel_allocation_t bit = 1 << channel; + + for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { + if (bit & channel_allocations[mode]) { + return mode; + } + } + + return -1; +} + +static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = io_timer_is_channel_free(channel); + + if (rv == 0) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; + channel_allocations[mode] |= bit; + } + + return rv; +} + + +static inline int free_channel_resource(unsigned channel) +{ + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[mode] &= ~bit; + channel_allocations[IOTimerChanMode_NotUsed] |= bit; + } + + return mode; +} + +int io_timer_free_channel(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_set_enable(false, mode, 1 << channel); + free_channel_resource(channel); + + } + + return 0; +} + + +static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = -EINVAL; + + if (mode != IOTimerChanMode_NotUsed) { + rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + rv = allocate_channel_resource(channel, mode); + } + } + + return rv; +} + +static int timer_set_rate(unsigned timer, unsigned rate) +{ +#if defined(CONFIG_ARCH_BOARD_CRAZYFLIE) + /* configure the timer to update at 328.125 kHz (recommended) */ + rARR(timer) = 255; +#else + /* configure the timer to update at the desired rate */ + rARR(timer) = 1000000 / rate; +#endif + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + return 0; +} + + +static int io_timer_init_timer(unsigned timer) +{ + /* Do this only once per timer */ + + int rv = is_timer_uninitalized(timer); + + if (rv == 0) { + + irqstate_t flags = px4_enter_critical_section(); + + set_timer_initalized(timer); + + /* enable the timer clock before we try to talk to it */ + + modifyreg32(io_timers[timer].clock_register, 0, io_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; + rCCR1(timer) = 0; + rCCR2(timer) = 0; + rCCR3(timer) = 0; + rCCR4(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) { + + /* master output enable = on */ + + rBDTR(timer) = ATIM_BDTR_MOE; + } + +#if defined(CONFIG_ARCH_BOARD_CRAZYFLIE) + /* configure the timer to free-run at timer frequency */ + rPSC(timer) = 0; +#else + /* configure the timer to free-run at 1MHz */ + + rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1; +#endif + + + /* + * Note we do the Standard PWM Out init here + * default to updating at 50Hz + */ + + timer_set_rate(timer, 50); + + /* + * Note that the timer is left disabled with IRQ subs installed + * and active but DEIR bits are not set. + */ + + irq_attach(io_timers[timer].vectorno, io_timers[timer].handler); + + up_enable_irq(io_timers[timer].vectorno); + + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_rate(unsigned timer, unsigned rate) +{ + /* Gather the channel bit that belong to the timer */ + + uint32_t channels = get_timer_channels(timer); + + /* Check ownership of PWM out */ + + if ((channels & channel_allocations[IOTimerChanMode_PWMOut]) != 0) { + + /* Change only a timer that is owned by pwm */ + + timer_set_rate(timer, rate); + } + + return 0; +} + +int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context) +{ + + uint32_t gpio = 0; + uint32_t clearbits = CCMR_C1_RESET; + uint32_t setbits = CCMR_C1_CAPTURE_INIT; + uint32_t ccer_setbits = GTIM_CCER_CC1E; + uint32_t dier_setbits = GTIM_DIER_CC1IE; + + /* figure out the GPIO config first */ + + switch (mode) { + case IOTimerChanMode_PWMOut: + ccer_setbits = 0; + dier_setbits = 0; + setbits = CCMR_C1_PWMOUT_INIT; + break; + + case IOTimerChanMode_PWMIn: + setbits = CCMR_C1_PWMIN_INIT; + gpio = timer_io_channels[channel].gpio_in; + break; + + case IOTimerChanMode_Capture: + setbits = CCMR_C1_CAPTURE_INIT; + gpio = timer_io_channels[channel].gpio_in; + break; + + case IOTimerChanMode_NotUsed: + setbits = 0; + break; + + default: + return -EINVAL; + } + + int rv = allocate_channel(channel, mode); + + /* Valid channel should now be reserved in new mode */ + + if (rv >= 0) { + + /* Blindly try to initialize the time - it will only do it once */ + + io_timer_init_timer(channels_timer(channel)); + + irqstate_t flags = px4_enter_critical_section(); + + /* Set up IO */ + if (gpio) { + px4_arch_configgpio(gpio); + } + + + unsigned timer = channels_timer(channel); + (void)timer; + + /* configure the channel */ + + uint32_t shifts = timer_io_channels[channel].timer_channel - 1; + + /* Map shifts timer channel 1-4 to 0-3 */ + + uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t)); + uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t)); + + clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; + setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; + + uint16_t rvalue = REG(timer, ccmr_offset); + rvalue &= ~clearbits; + rvalue |= setbits; + REG(timer, ccmr_offset) = rvalue; + + /* + * The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode + * As soon as CCxS (in SSMRx becomes different from 00, the channel is configured + * in input and the TIMx_CCR1 register becomes read-only. + * so the next line does nothing in capture mode and initializes an PWM out to + * 0 + */ + + REG(timer, ccr_offset) = 0; + + /* on PWM Out ccer_setbits is 0 */ + + clearbits = 0;///(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS); + setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS); + rvalue = rCCER(timer); + rvalue &= ~clearbits; + rvalue |= setbits; + rCCER(timer) = rvalue; + + channel_handlers[channel].callback = channel_handler; + channel_handlers[channel].context = context; + rDIER(timer) |= dier_setbits << shifts; + px4_leave_critical_section(flags); + } + + return rv; +} + +int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks) +{ + + struct action_cache_t { + uint32_t ccer_clearbits; + uint32_t ccer_setbits; + uint32_t dier_setbits; + uint32_t dier_clearbits; + uint32_t base; + uint32_t gpio[MAX_CHANNELS_PER_TIMER]; + } action_cache[MAX_IO_TIMERS]; + memset(action_cache, 0, sizeof(action_cache)); + + uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0; + uint32_t ccer_bit = state ? GTIM_CCER_CC1E : 0; + + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_PWMOut: + dier_bit = 0; + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + break; + + default: + return -EINVAL; + } + + /* Was the request for all channels in this mode ?*/ + + if (masks == IO_TIMER_ALL_MODES_CHANNELS) { + + /* Yes - we provide them */ + + masks = channel_allocations[mode]; + + } else { + + /* No - caller provided mask */ + + /* Only allow the channels in that mode to be affected */ + + masks &= channel_allocations[mode]; + + } + + /* Pre calculate all the changes */ + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1; + uint32_t timer = channels_timer(chan_index); + action_cache[timer].base = io_timers[timer].base; + action_cache[timer].ccer_clearbits |= GTIM_CCER_CC1E << (shifts * CCER_C1_NUM_BITS); + action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS); + action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts; + action_cache[timer].dier_setbits |= dier_bit << shifts; + + if (state && mode == IOTimerChanMode_PWMOut) { + action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; + } + } + } + + irqstate_t flags = px4_enter_critical_section(); + + for (unsigned actions = 0; actions < arraySize(action_cache) && action_cache[actions].base != 0 ; actions++) { + uint32_t rvalue = 0;//_REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET); + rvalue &= ~action_cache[actions].ccer_clearbits; + rvalue |= action_cache[actions].ccer_setbits; + //_REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue; + uint32_t after = rvalue ;//& (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); + + rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET); + rvalue &= ~action_cache[actions].dier_clearbits; + rvalue |= action_cache[actions].dier_setbits; + _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue; + + + /* Any On ?*/ + + if (after != 0) { + + /* force an update to preload all registers */ + rEGR(actions) = GTIM_EGR_UG; + + for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) { + if (action_cache[actions].gpio[chan]) { + px4_arch_configgpio(action_cache[actions].gpio[chan]); + action_cache[actions].gpio[chan] = 0; + } + } + + /* arm requires the timer be enabled */ + rCR1(actions) = 0; // |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + + rCR1(actions) = 0; + } + } + + px4_leave_critical_section(flags); + + return 0; +} + +int io_timer_set_ccr(unsigned channel, uint16_t value) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (io_timer_get_channel_mode(channel) != IOTimerChanMode_PWMOut) { + + rv = -EIO; + + } else { + + /* configure the channel */ + + if (value > 0) { + value--; + } + + REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value; + } + } + + return rv; +} + +uint16_t io_channel_get_ccr(unsigned channel) +{ + uint16_t value = 0; + + if (io_timer_validate_channel_index(channel) == 0 && + io_timer_get_channel_mode(channel) == IOTimerChanMode_PWMOut) { + value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) + 1; + } + + return value; +} + +uint32_t io_timer_get_group(unsigned timer) +{ + return get_timer_channels(timer); + +} diff --git a/src/drivers/kinetis/drv_io_timer.h b/src/drivers/kinetis/drv_io_timer.h new file mode 100644 index 0000000000..d8d7ce71e9 --- /dev/null +++ b/src/drivers/kinetis/drv_io_timer.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_io_timer.h + * + * stm32-specific PWM output data. + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#define MAX_IO_TIMERS 4 +#define MAX_TIMER_IO_CHANNELS 8 + +#define MAX_LED_TIMERS 1 +#define MAX_TIMER_LED_CHANNELS 3 + +#define IO_TIMER_ALL_MODES_CHANNELS 0 + +typedef enum io_timer_channel_mode_t { + IOTimerChanMode_NotUsed = 0, + IOTimerChanMode_PWMOut = 1, + IOTimerChanMode_PWMIn = 2, + IOTimerChanMode_Capture = 3, + IOTimerChanModeSize +} io_timer_channel_mode_t; + +typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */ + +/* array of timers dedicated to PWM in and out and capture use */ +typedef struct io_timers_t { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; + uint32_t vectorno; + uint32_t first_channel_index; + uint32_t last_channel_index; + xcpt_t handler; +} io_timers_t; + +/* array of channels in logical order */ +typedef struct timer_io_channels_t { + uint32_t gpio_out; + uint32_t gpio_in; + uint8_t timer_index; + uint8_t timer_channel; + uint16_t masks; + uint8_t ccr_offset; +} timer_io_channels_t; + + +typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt); + + +/* supplied by board-specific code */ +__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS]; +__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS]; + +__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; +__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; + +__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; +__EXPORT int io_timer_handler0(int irq, void *context); +__EXPORT int io_timer_handler1(int irq, void *context); +__EXPORT int io_timer_handler2(int irq, void *context); +__EXPORT int io_timer_handler3(int irq, void *context); + +__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context); +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, + io_timer_channel_allocation_t masks); +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT uint16_t io_channel_get_ccr(unsigned channel); +__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); +__EXPORT uint32_t io_timer_get_group(unsigned timer); +__EXPORT int io_timer_validate_channel_index(unsigned channel); +__EXPORT int io_timer_is_channel_free(unsigned channel); +__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_get_channel_mode(unsigned channel); +__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); +__END_DECLS diff --git a/src/drivers/kinetis/drv_led_pwm.cpp b/src/drivers/kinetis/drv_led_pwm.cpp new file mode 100644 index 0000000000..263ca6645c --- /dev/null +++ b/src/drivers/kinetis/drv_led_pwm.cpp @@ -0,0 +1,318 @@ +/**************************************************************************** + * + * 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 + + +#include + +#if defined(BOARD_HAS_LED_PWM) +#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); + + +static void +led_pwm_timer_init(unsigned timer) +{ + /* valid Timer */ + + if (led_pwm_timers[timer].base != 0) { + + /* 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 */ + + px4_arch_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; + rCCER(timer) |= GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCER(timer) |= GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCER(timer) |= GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCER(timer) |= GTIM_CCER_CC4E; + break; + } +} + +int +led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= arraySize(led_pwm_channels)) { + 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); + +#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) + unsigned value = period - (unsigned)cvalue * period / 255; +#else + unsigned value = (unsigned)cvalue * period / 255; +#endif + + /* 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 < arraySize(led_pwm_timers); i++) { + led_pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < arraySize(led_pwm_channels); 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 < arraySize(led_pwm_timers); 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 { + rCR1(i) = 0; + } + } + } +} + +#endif // BOARD_HAS_LED_PWM diff --git a/src/drivers/kinetis/drv_pwm_servo.c b/src/drivers/kinetis/drv_pwm_servo.c new file mode 100644 index 0000000000..dfe4147886 --- /dev/null +++ b/src/drivers/kinetis/drv_pwm_servo.c @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drv_io_timer.h" +#include "drv_pwm_servo.h" + +#include + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_free_channel(channel); + current &= ~(1 << channel); + } + } + + // Now allocate the new set + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not PWM mode before + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + return OK; +} + +void up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) +{ + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) { + return -ERANGE; + } + + if (rate > 10000) { + return -ERANGE; + } + + if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) { + return ERROR; + } + + io_timer_set_rate(group, rate); + + return OK; +} + +int up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { + up_pwm_servo_set_rate_group_update(i, rate); + } + + return 0; +} + +uint32_t up_pwm_servo_get_rate_group(unsigned group) +{ + return io_timer_get_group(group); +} + +void +up_pwm_servo_arm(bool armed) +{ + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/src/drivers/kinetis/drv_pwm_servo.h b/src/drivers/kinetis/drv_pwm_servo.h new file mode 100644 index 0000000000..e3477183d2 --- /dev/null +++ b/src/drivers/kinetis/drv_pwm_servo.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_pwm_servo.h + * + * stm32-specific PWM output data. + */ + +#pragma once + +#include diff --git a/src/drivers/kinetis/tone_alarm/CMakeLists.txt b/src/drivers/kinetis/tone_alarm/CMakeLists.txt new file mode 100644 index 0000000000..cf58f2d5c1 --- /dev/null +++ b/src/drivers/kinetis/tone_alarm/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2016 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__kinetis__tone_alarm + MAIN tone_alarm + COMPILE_FLAGS + SRCS + tone_alarm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/kinetis/tone_alarm/tone_alarm.cpp b/src/drivers/kinetis/tone_alarm/tone_alarm.cpp new file mode 100644 index 0000000000..a58680c9f0 --- /dev/null +++ b/src/drivers/kinetis/tone_alarm/tone_alarm.cpp @@ -0,0 +1,1019 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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. + * + ****************************************************************************/ + +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * + * The tone_alarm driver supports a set of predefined "alarm" + * tunes and one user-supplied tune. + * + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm tune, from 1 - . Selecting tune zero silences + * the alarm. + * + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +/* TODO:Determin the correct type of timer to use. + * TODO:This is stubbed out leving the code intact to document the needed + * mechinsm for porting. + */ + +//#include + +#include +#include + +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) +# if TONE_ALARM_TIMER == HRT_TIMER +# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. +# endif +#endif + +# define TONE_ALARM_CLOCK 1000000 + +#if TONE_ALARM_TIMER == 1 +//# define TONE_ALARM_BASE STM32_TIM1_BASE +//# define TONE_ALARM_CLOCK 1000000 +//# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +//# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN +//# ifdef CONFIG_STM32_TIM1 +//# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 +//# endif +//#elif TONE_ALARM_TIMER == 2 +#else +//# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver. +#endif + +#if TONE_ALARM_CHANNEL == 1 +# define TONE_CCMR1 (3 << 4) +# define TONE_CCMR2 0 +# define TONE_CCER (1 << 0) +# define TONE_rCCR rCCR1 +#elif TONE_ALARM_CHANNEL == 2 +# define TONE_CCMR1 (3 << 12) +# define TONE_CCMR2 0 +# define TONE_CCER (1 << 4) +# define TONE_rCCR rCCR2 +#elif TONE_ALARM_CHANNEL == 3 +# define TONE_CCMR1 0 +# define TONE_CCMR2 (3 << 4) +# define TONE_CCER (1 << 8) +# define TONE_rCCR rCCR3 +#elif TONE_ALARM_CHANNEL == 4 +# define TONE_CCMR1 0 +# define TONE_CCMR2 (3 << 12) +# define TONE_CCER (1 << 12) +# define TONE_rCCR rCCR4 +#else +# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver. +#endif + +/* + * Timer register accessors + */ +static volatile uint32_t dummy[18]; +#define REG(_reg) (*(volatile uint32_t *)(&dummy[(_reg)])) +#undef modifyreg32 +#define modifyreg32(reg,clr,set) +#define GTIM_CR1_CEN 0 +#define GTIM_EGR_UG 0 + +# define rCR1 REG(0) +# define rCR2 REG(1) +# define rSMCR REG(2) +# define rDIER REG(3) +# define rSR REG(4) +# define rEGR REG(5) +# define rCCMR1 REG(6) +# define rCCMR2 REG(7) +# define rCCER REG(8) +# define rCNT REG(9) +# define rPSC REG(10) +# define rARR REG(11) +# define rCCR1 REG(12) +# define rCCR2 REG(13) +# define rCCR3 REG(14) +# define rCCR4 REG(15) +# define rDCR REG(16) +# define rDMAR REG(17) + +#define CBRK_BUZZER_KEY 782097 + +class ToneAlarm : public device::CDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) + { + return _tune_names[tune]; + } + + enum { + CBRK_OFF = 0, + CBRK_ON, + CBRK_UNINIT + }; + +private: + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; + static const uint8_t _note_tab[]; + + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + int _cbrk; //if true, no audio output + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // + static void next_trampoline(void *arg); + +}; + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + + +ToneAlarm::ToneAlarm() : + CDev("tone_alarm", TONEALARM0_DEVICE_PATH), + _default_tune_number(0), + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr), + _cbrk(CBRK_UNINIT) +{ + // enable debug() calls + //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } + + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); + return; + } + + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + + if (c == 0) { + goto tune_end; + } + + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + + if (_note_length < 1) { + goto tune_error; + } + + break; + + case 'O': // select octave + _octave = next_number(); + + if (_octave > 6) { + _octave = 6; + } + + break; + + case '<': // decrease octave + if (_octave > 0) { + _octave--; + } + + break; + + case '>': // increase octave + if (_octave < 6) { + _octave++; + } + + break; + + case 'M': // select inter-note gap + c = next_char(); + + if (c == 0) { + goto tune_error; + } + + _next++; + + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + + case 'L': + _note_mode = MODE_LEGATO; + break; + + case 'S': + _note_mode = MODE_STACCATO; + break; + + case 'F': + _repeat = false; + break; + + case 'B': + _repeat = true; + break; + + default: + goto tune_error; + } + + break; + + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; + } + + case 'N': // play an arbitrary note + note = next_number(); + + if (note > 84) { + goto tune_error; + } + + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + + break; + + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) { + note++; + } + + _next++; + break; + + case '-': // down a semitone + if (note > 1) { + note--; + } + + _next++; + break; + + default: + // 0 / no next char here is OK + break; + } + + // shorthand length notation + note_length = next_number(); + + if (note_length == 0) { + note_length = _note_length; + } + + break; + + default: + goto tune_error; + } + } + + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; + + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + syslog(LOG_ERR, "tune error\n"); + _repeat = false; // don't loop on error + + // stop (and potentially restart) the tune +tune_end: + stop_note(); + + if (_repeat) { + start_tune(_tune); + + } else { + _tune = nullptr; + _default_tune_number = 0; + } + + return; +} + +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; + } + + return toupper(*_next); +} + +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + + if (!isdigit(c)) { + return number; + } + + _next++; + number = (number * 10) + (c - '0'); + } +} + +unsigned +ToneAlarm::next_dots() +{ + unsigned dots = 0; + + while (next_char() == '.') { + _next++; + dots++; + } + + return dots; +} + +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + DEVICE_DEBUG("ioctl %i %u", cmd, arg); + +// irqstate_t flags = px4_enter_critical_section(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + DEVICE_DEBUG("TONE_SET_ALARM %u", arg); + + if (arg < TONE_NUMBER_OF_TUNES) { + if (arg == TONE_STOP_TUNE) { + // stop the tune + _tune = nullptr; + _next = nullptr; + _repeat = false; + _default_tune_number = 0; + + } else { + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ + _default_tune_number = arg; + start_tune(_default_tunes[arg]); + } + } + + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; + } + +// px4_leave_critical_section(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) { + result = CDev::ioctl(filp, cmd, arg); + } + + return result; +} + +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) +{ + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) { + return -EFBIG; + } + + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') { + return OK; + } + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + + if (_user_tune == nullptr) { + return -ENOMEM; + } + + // and play it + start_tune(_user_tune); + + return len; +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_tune(unsigned tune) +{ + int fd, ret; + + fd = open(TONEALARM0_DEVICE_PATH, 0); + + if (fd < 0) { + err(1, TONEALARM0_DEVICE_PATH); + } + + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); + + if (ret != 0) { + err(1, "TONE_SET_ALARM"); + } + + exit(0); +} + +int +play_string(const char *str, bool free_buffer) +{ + int fd, ret; + + fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + + if (fd < 0) { + err(1, TONEALARM0_DEVICE_PATH); + } + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (free_buffer) { + free((void *)str); + } + + if (ret < 0) { + err(1, "play tune"); + } + + exit(0); +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned tune; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) { + errx(1, "couldn't allocate the ToneAlarm driver"); + } + + if (g_dev->init() != OK) { + delete g_dev; + errx(1, "ToneAlarm init failed"); + } + } + + if (argc > 1) { + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) { + play_tune(TONE_STOP_TUNE); + } + + if (!strcmp(argv1, "stop")) { + play_tune(TONE_STOP_TUNE); + } + + if ((tune = strtol(argv1, nullptr, 10)) != 0) { + play_tune(tune); + } + + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv1)) { + play_tune(tune); + } + + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + + if (fd == nullptr) { + errx(1, "couldn't open '%s'", argv1); + } + + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + + if (buffer == nullptr) { + errx(1, "not enough memory memory"); + } + + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } + + /* if it looks like a PLAY string... */ + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); + } + } + + } + + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); +}