diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 7b6d6753fed..dd0806e1522 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -37,6 +37,7 @@ pipeline { "modalai_fc-v1_default", "mro_x21_default", "mro_ctrl-zero-f7_default", "mro_x21-777_default", "nxp_fmuk66-v3_default", + "nxp_fmurt1062-v1_default", "omnibus_f4sd_default", "px4_fmu-v2_default", "px4_fmu-v2_fixedwing", "px4_fmu-v2_lpe", "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v2_test", "px4_fmu-v3_default", diff --git a/Makefile b/Makefile index e4cd7160001..bfb27ea7b49 100644 --- a/Makefile +++ b/Makefile @@ -253,6 +253,7 @@ px4fmu_firmware: \ misc_qgc_extra_firmware: \ check_nxp_fmuk66-v3_default \ + check_nxp_fmurt1062-v1_default \ check_intel_aerofc-v1_default \ check_mro_x21_default \ check_bitcraze_crazyflie_default \ diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake new file mode 100644 index 00000000000..4641540a877 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -0,0 +1,125 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR nxp + MODEL fmurt1062-v1 + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + TESTING +# UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS1 + TEL1:/dev/ttyS3 + TEL2:/dev/ttyS2 + GPS2:/dev/ttyS4 + + DRIVERS + adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers +# dshot not ported + gps + #heater + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/bmi055 + imu/mpu6000 + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + mkblctrl + optical_flow # all available optical flow drivers + #osd + pca9685 + power_monitor/ina226 + #protocol_splitter +# pwm_input - not ptorable + pwm_out_sim + px4fmu + rc_input + roboclaw + safety_button + tap_esc + telemetry # all available telemetry drivers +# test_ppm not portable + tone_alarm +# uavcan + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + vmount + vtol_att_control + SYSTEMCMDS + bl_update + config + dmesg + dumpfile + esc_calib + #hardfault_log # Needs bbsrm + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + shutdown + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + serial_test + EXAMPLES + bottle_drop # OBC challenge + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + ) diff --git a/boards/nxp/fmurt1062-v1/firmware.prototype b/boards/nxp/fmurt1062-v1/firmware.prototype new file mode 100644 index 00000000000..cbb26388e0b --- /dev/null +++ b/boards/nxp/fmurt1062-v1/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 28, + "magic": "PX4FWv1", + "description": "Firmware for the NXPFMURT1062v1 board", + "image": "", + "build_time": 0, + "summary": "NXPFMURT1062v1", + "version": "0.1", + "image_size": 0, + "image_maxsize": 8388608, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_defaults b/boards/nxp/fmurt1062-v1/init/rc.board_defaults new file mode 100644 index 00000000000..0a44f6e6da1 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/init/rc.board_defaults @@ -0,0 +1,14 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board defaults +#------------------------------------------------------------------------------ + + +if [ $AUTOCNF = yes ] +then + +fi + +set LOGGER_BUF 64 + +safety_button start diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_mavlink b/boards/nxp/fmurt1062-v1/init/rc.board_mavlink new file mode 100644 index 00000000000..2ec618d05e3 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the USB port +mavlink start -d /dev/ttyACM0 diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors new file mode 100644 index 00000000000..fe8f3670195 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -0,0 +1,58 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board sensors init +#------------------------------------------------------------------------------ +# +# UART mapping on NXP FMURT1062: +# +# LPUART7 /dev/ttyS0 CONSOLE +# LPUART2 /dev/ttyS1 GPS +# LPUART3 /dev/ttyS2 TELEM2 (GPIO flow control) +# LPUART4 /dev/ttyS3 TELEM1 (UART flow control) +# LPUART5 /dev/ttyS4 TELEM4 GPS2 +# LPUART6 /dev/ttyS5 TELEM3 (RC_INPUT) +# LPUART8 /dev/ttyS6 PX4IO +# +#------------------------------------------------------------------------------ + +adc start + +# Internal SPI bus ICM-20602 +mpu6000 -R 8 -s -T 20602 start + +# Internal SPI bus ICM-20689 +mpu6000 -R 8 -z -T 20689 start + +# Internal SPI bus BMI055 accel +bmi055 -A -R 10 start + +# Internal SPI bus BMI055 gyro +bmi055 -G -R 10 start + +# Possible external compasses +ist8310 -C -b 1 start +ist8310 -C -b 2 start +hmc5883 -C -T -X start +qmc5883 -X start +lis3mdl -X start + +# ICM20948 as external magnetometer on I2C (e.g. Here GPS) +if ! icm20948 -X -M -R 6 start +then + # external emulated AK09916 (Here2) is rotated 270 degrees yaw + ak09916 -X -R 6 start +fi + +# Possible internal compass +ist8310 -C -b 5 start + +# Baro on internal SPI +ms5611 -s start + +# External RM3100 (I2C or SPI) +rm3100 start + +# Possible pmw3901 optical flow sensor +pmw3901 start + +px4flow start & diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig b/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig new file mode 100644 index 00000000000..433c13fcad4 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig @@ -0,0 +1,32 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +choice + prompt "Boot Flash" + default NXP_FMURT1062_V3_QSPI_FLASH + +config NXP_FMURT1062_V3_HYPER_FLASH + bool "HYPER Flash" + +config NXP_FMURT1062_V3_QSPI_FLASH + bool "QSPI Flash" + +endchoice # Boot Flash + +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-8, as PROBE_1-8 to provide timing signals + from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-8 as PROBE_1-8" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-8, as PROBE_1-8 to provide timing signals + from selected drivers. diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h b/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h new file mode 100644 index 00000000000..03976828fa3 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h @@ -0,0 +1,391 @@ +/************************************************************************************ + * nuttx-configs/nxp_fmurt1062-v1/include/board.h + * + * Copyright (C) 2018 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. + * + ************************************************************************************/ + +#ifndef __NUTTX_CONFIG_NXP_FMURT1062_V1_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_NXP_FMURT1062_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* Set VDD_SOC to 1.3V */ + +#define IMXRT_VDD_SOC (0x14) + +/* Set Arm PLL (PLL1) to fOut = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * 576Mhz = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * ARM_PLL_DIV_SELECT = 96 + * ARM_PODF_DIVISOR = 2 + * 576Mhz = (24Mhz * 96/2) / 2 + * + * AHB_CLOCK_ROOT = PLL1fOut / IMXRT_AHB_PODF_DIVIDER + * 1Hz to 600 Mhz = 576Mhz / IMXRT_ARM_CLOCK_DIVIDER + * IMXRT_ARM_CLOCK_DIVIDER = 1 + * 576Mhz = 576Mhz / 1 + * + * PRE_PERIPH_CLK_SEL = PRE_PERIPH_CLK_SEL_PLL1 + * PERIPH_CLK_SEL = 1 (0 select PERIPH_CLK2_PODF, 1 select PRE_PERIPH_CLK_SEL_PLL1) + * PERIPH_CLK = 576Mhz + * + * IPG_CLOCK_ROOT = AHB_CLOCK_ROOT / IMXRT_IPG_PODF_DIVIDER + * IMXRT_IPG_PODF_DIVIDER = 4 + * 144Mhz = 576Mhz / 4 + * + * PRECLK_CLOCK_ROOT = IPG_CLOCK_ROOT / IMXRT_PERCLK_PODF_DIVIDER + * IMXRT_PERCLK_PODF_DIVIDER = 1 + * 16Mhz = 144Mhz / 9 + * + * SEMC_CLK_ROOT = 576Mhz / IMXRT_SEMC_PODF_DIVIDER (labeled AIX_PODF in 18.2) + * IMXRT_SEMC_PODF_DIVIDER = 8 + * 72Mhz = 576Mhz / 8 + * + * Set Sys PLL (PLL2) to fOut = (24Mhz * (20+(2*(DIV_SELECT))) + * 528Mhz = (24Mhz * (20+(2*(1))) + * + * Set USB1 PLL (PLL3) to fOut = (24Mhz * 20) + * 480Mhz = (24Mhz * 20) + * + * Set LPSPI PLL3 PFD0 to fOut = (480Mhz / 12 * 18) + * 720Mhz = (480Mhz / 12 * 18) + * 90Mhz = (720Mhz / LSPI_PODF_DIVIDER) + * + * Set LPI2C PLL3 / 8 to fOut = (480Mhz / 8) + * 60Mhz = (480Mhz / 8) + * 12Mhz = (60Mhz / LSPI_PODF_DIVIDER) + * + * Set USDHC1 PLL2 PFD2 to fOut = (528Mhz / 24 * 18) + * 396Mhz = (528Mhz / 24 * 18) + * 198Mhz = (396Mhz / IMXRT_USDHC1_PODF_DIVIDER) + */ + +#define BOARD_XTAL_FREQUENCY 24000000 +#define IMXRT_PRE_PERIPH_CLK_SEL CCM_CBCMR_PRE_PERIPH_CLK_SEL_PLL1 +#define IMXRT_PERIPH_CLK_SEL CCM_CBCDR_PERIPH_CLK_SEL_PRE_PERIPH +#define IMXRT_ARM_PLL_DIV_SELECT 96 +#define IMXRT_ARM_PODF_DIVIDER 2 +#define IMXRT_AHB_PODF_DIVIDER 1 +#define IMXRT_IPG_PODF_DIVIDER 4 +#define IMXRT_PERCLK_CLK_SEL CCM_CSCMR1_PERCLK_CLK_SEL_IPG_CLK_ROOT +#define IMXRT_PERCLK_PODF_DIVIDER 9 +#define IMXRT_SEMC_PODF_DIVIDER 8 + +#define IMXRT_LPSPI_CLK_SELECT CCM_CBCMR_LPSPI_CLK_SEL_PLL3_PFD0 +#define IMXRT_LSPI_PODF_DIVIDER 8 + +#define IMXRT_LPI2C_CLK_SELECT CCM_CSCDR2_LPI2C_CLK_SEL_PLL3_60M +#define IMXRT_LSI2C_PODF_DIVIDER 5 + +#define IMXRT_USDHC1_CLK_SELECT CCM_CSCMR1_USDHC1_CLK_SEL_PLL2_PFD0 +#define IMXRT_USDHC1_PODF_DIVIDER 2 + +#define IMXRT_SYS_PLL_SELECT CCM_ANALOG_PLL_SYS_DIV_SELECT_22 + +#define BOARD_CPU_FREQUENCY \ + (BOARD_XTAL_FREQUENCY * (IMXRT_ARM_PLL_DIV_SELECT / 2)) / IMXRT_ARM_PODF_DIVIDER + +#define BOARD_GPT_FREQUENCY \ + (BOARD_CPU_FREQUENCY / IMXRT_IPG_PODF_DIVIDER) / IMXRT_PERCLK_PODF_DIVIDER + +/* Define this to enable tracing */ +#if CONFIG_USE_TRACE +# define IMXRT_TRACE_PODF_DIVIDER 1 +# define IMXRT_TRACE_CLK_SELECT CCM_CBCMR_TRACE_CLK_SEL_PLL2_PFD0 +#endif + +/* SDIO *****************************************************************************/ + +/* Pin drive characteristics */ + +#define USDHC1_DATAX_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CMD_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CLK_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_SPEED_MAX) +#define USDHC1_CD_IOMUX (IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) + +#define PIN_USDHC1_D0 (GPIO_USDHC1_DATA0_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_02 */ +#define PIN_USDHC1_D1 (GPIO_USDHC1_DATA1_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_03 */ +#define PIN_USDHC1_D2 (GPIO_USDHC1_DATA2_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_04 */ +#define PIN_USDHC1_D3 (GPIO_USDHC1_DATA3_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_05 */ +#define PIN_USDHC1_DCLK (GPIO_USDHC1_CLK_1 | USDHC1_CLK_IOMUX) /* GPIO_SD_B0_01 */ +#define PIN_USDHC1_CMD (GPIO_USDHC1_CMD_1 | USDHC1_CMD_IOMUX) /* GPIO_SD_B0_00 */ +#define PIN_USDHC1_CD (GPIO_USDHC1_CD_2 | USDHC1_CD_IOMUX) /* GPIO_B1_12 */ + +/* Ideal 400Khz for initial inquiry. + * Given input clock 198 Mhz. + * 386.71875 KHz = 198 Mhz / (256 * 2) + */ + +#define BOARD_USDHC_IDMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV256 +#define BOARD_USDHC_IDMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(2) + +/* Ideal 25 Mhz for other modes + * Given input clock 198 Mhz. + * 24.75 MHz = 198 Mhz / (8 * 1) + */ + +#define BOARD_USDHC_MMCMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_MMCMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD1MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD1MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD4MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD4MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +/* LED definitions ******************************************************************/ +/* The nxp fmutr1062 board has numerous LEDs but only three, LED_GREEN a Green LED, + * LED_BLUE a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * 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_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* PIO Disambiguation ***************************************************************/ +/* LPUARTs + */ +#define LPUART_IOMUX (IOMUX_PULL_UP_22K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW | IOMUX_SPEED_LOW | IOMUX_SCHMITT_TRIGGER) + +/* GPS 1 */ + +#define GPIO_LPUART2_RX (GPIO_LPUART2_RX_1 | LPUART_IOMUX) /* EVK J22-8 */ /* GPIO_AD_B1_03 */ +#define GPIO_LPUART2_TX (GPIO_LPUART2_TX_1 | LPUART_IOMUX) /* EVK J22-7 */ /* GPIO_AD_B1_02 */ + +/* Telem 2 */ + +#define HS_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_SLEW_SLOW | IOMUX_DRIVE_HIZ | IOMUX_SPEED_MEDIUM | IOMUX_PULL_UP_47K) +#define HS_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_PULL_KEEP) + +#define GPIO_LPUART3_RX (GPIO_LPUART3_RX_3 | LPUART_IOMUX) /* GPIO_B0_09 */ +#define GPIO_LPUART3_TX (GPIO_LPUART3_TX_3 | LPUART_IOMUX) /* GPIO_B0_08 */ +#define GPIO_LPUART3_CTS (GPIO_PORT3 | GPIO_PIN4 | GPIO_INPUT | HS_INPUT_IOMUX) /* GPIO_SD_B1_04 GPIO3_IO04 (GPIO only, no HW Flow control) */ +#define GPIO_LPUART3_RTS (GPIO_PORT4 | GPIO_PIN24 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | HS_OUTPUT_IOMUX) /* GPIO_EMC_24 GPIO4_IO24 (GPIO only, no HW Flow control) */ + +/* Telem 1 */ + +#define GPIO_LPUART4_RX (GPIO_LPUART4_RX_2 | LPUART_IOMUX) /* GPIO_EMC_20 */ +#define GPIO_LPUART4_TX (GPIO_LPUART4_TX_2 | LPUART_IOMUX) /* GPIO_EMC_19 */ +#define GPIO_LPUART4_CTS (GPIO_LPUART4_CTS_1 | LPUART_IOMUX) /* GPIO_EMC_17 */ +#define GPIO_LPUART4_RTS (GPIO_LPUART4_RTS_1 | LPUART_IOMUX) /* GPIO_EMC_18 */ + +/* GPS2 */ + +#define GPIO_LPUART5_RX (GPIO_LPUART5_RX_1 | LPUART_IOMUX) /* GPIO_B1_13 */ +#define GPIO_LPUART5_TX (GPIO_LPUART5_TX_2 | LPUART_IOMUX) /* GPIO_EMC_23 */ + +/* RC INPUT single wire mode on TX, RX is not used */ + +#define GPIO_LPUART6_RX (GPIO_LPUART6_RX_2 | LPUART_IOMUX) /* GPIO_EMC_26 */ +#define GPIO_LPUART6_TX (GPIO_LPUART6_TX_2 | LPUART_IOMUX) /* GPIO_EMC_25 */ + +#define GPIO_LPUART7_RX (GPIO_LPUART7_RX_1 | LPUART_IOMUX) /* GPIO_EMC_32 */ +#define GPIO_LPUART7_TX (GPIO_LPUART7_TX_1 | LPUART_IOMUX) /* GPIO_EMC_31 */ + +#define GPIO_LPUART8_RX (GPIO_LPUART8_RX_2 | LPUART_IOMUX) /* GPIO_EMC_39 */ +#define GPIO_LPUART8_TX (GPIO_LPUART8_TX_2 | LPUART_IOMUX) /* GPIO_EMC_38 */ + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + * CAN3 is routed to transceiver. + */ +#define FLEXCAN_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MEDIUM) + +#define GPIO_FLEXCAN1_RX (GPIO_FLEXCAN1_RX_2 | FLEXCAN_IOMUX) /* GPIO_B0_03 */ +#define GPIO_FLEXCAN1_TX (GPIO_FLEXCAN1_TX_4 | FLEXCAN_IOMUX) /* GPIO_SD_B1_02 */ +#define GPIO_FLEXCAN2_RX (GPIO_FLEXCAN2_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_03 */ +#define GPIO_FLEXCAN2_TX (GPIO_FLEXCAN2_TX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_02 */ +#define GPIO_FLEXCAN3_RX (GPIO_FLEXCAN3_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_11 */ +#define GPIO_FLEXCAN3_TX (GPIO_FLEXCAN3_TX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_36 */ + +/* LPSPI */ +#define LPSPI_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MAX) + +#define GPIO_LPSPI1_SCK (GPIO_LPSPI1_SCK_1 | LPSPI_IOMUX) /* GPIO_EMC_27 */ +#define GPIO_LPSPI1_MISO (GPIO_LPSPI1_SDI_1 | LPSPI_IOMUX) /* GPIO_EMC_29 */ +#define GPIO_LPSPI1_MOSI (GPIO_LPSPI1_SDO_1 | LPSPI_IOMUX) /* GPIO_EMC_28 */ + +#define GPIO_LPSPI2_SCK (GPIO_LPSPI2_SCK_1 | LPSPI_IOMUX) /* GPIO_EMC_00 */ +#define GPIO_LPSPI2_MISO (GPIO_LPSPI2_SDI_1 | LPSPI_IOMUX) /* GPIO_EMC_03 */ +#define GPIO_LPSPI2_MOSI (GPIO_LPSPI2_SDO_1 | LPSPI_IOMUX) /* GPIO_EMC_02 */ + +#define GPIO_LPSPI3_SCK (GPIO_LPSPI3_SCK_1 | LPSPI_IOMUX) /* GPIO_AD_B1_15 */ +#define GPIO_LPSPI3_MISO (GPIO_LPSPI3_SDI_1 | LPSPI_IOMUX) /* GPIO_AD_B1_13 */ +#define GPIO_LPSPI3_MOSI (GPIO_LPSPI3_SDO_1 | LPSPI_IOMUX) /* GPIO_AD_B1_14 */ + +#define GPIO_LPSPI4_SCK (GPIO_LPSPI4_SCK_1 | LPSPI_IOMUX) /* GPIO_B1_07 */ +#define GPIO_LPSPI4_MISO (GPIO_LPSPI4_SDI_1 | LPSPI_IOMUX) /* GPIO_B1_05 */ +#define GPIO_LPSPI4_MOSI (GPIO_LPSPI4_SDO_2 | LPSPI_IOMUX) /* GPIO_B0_02 */ + +/* LPI2Cs */ + +#define LPI2C_IOMUX (IOMUX_SPEED_MEDIUM | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | GPIO_SION_ENABLE) +#define LPI2C_IO_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | IOMUX_PULL_NONE) + +#define GPIO_LPI2C1_SDA (GPIO_LPI2C1_SDA_2 | LPI2C_IOMUX) /* EVK J24-9 R276 */ /* GPIO_AD_B1_01 */ +#define GPIO_LPI2C1_SCL (GPIO_LPI2C1_SCL_2 | LPI2C_IOMUX) /* EVK J24-10 R277 */ /* GPIO_AD_B1_00 */ + +#define GPIO_LPI2C1_SDA_RESET (GPIO_PORT1 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_01 GPIO1_IO17 */ +#define GPIO_LPI2C1_SCL_RESET (GPIO_PORT1 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_00 GPIO1_IO16 */ + +#define GPIO_LPI2C2_SDA (GPIO_LPI2C2_SDA_1 | LPI2C_IOMUX) /* EVK J8-A25 */ /* GPIO_B0_05 */ +#define GPIO_LPI2C2_SCL (GPIO_LPI2C2_SCL_1 | LPI2C_IOMUX) /* EVK J8-A24 */ /* GPIO_B0_04 */ + +#define GPIO_LPI2C2_SDA_RESET (GPIO_PORT2 | GPIO_PIN5 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_B0_05 GPIO2_IO5 */ +#define GPIO_LPI2C2_SCL_RESET (GPIO_PORT2 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_B0_04 GPIO2_IO4 */ + +#define GPIO_LPI2C3_SDA (GPIO_LPI2C3_SDA_2 | LPI2C_IOMUX) /* GPIO_EMC_21 */ +#define GPIO_LPI2C3_SCL (GPIO_LPI2C3_SCL_2 | LPI2C_IOMUX) /* GPIO_EMC_22 */ + +#define GPIO_LPI2C3_SDA_RESET (GPIO_PORT4 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_EMC_21 GPIO4_IO21 */ +#define GPIO_LPI2C3_SCL_RESET (GPIO_PORT4 | GPIO_PIN22 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_EMC_22 GPIO4_IO22 */ + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +#include +#include +// add -I build/nxp_fmurt1062-v1_default/NuttX/nuttx/arch/arm/src/chip \ to NuttX Makedefs.in +#define PROBE_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 /* GPIO_B0_06 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_2 /* GPIO_EMC_08 */ (GPIO_PORT4 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_3 /* GPIO_EMC_10 */ (GPIO_PORT4 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_4 /* GPIO_AD_B0_09 */ (GPIO_PORT1 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_5 /* GPIO_EMC_33 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_6 /* GPIO_EMC_30 */ (GPIO_PORT4 | GPIO_PIN30 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_7 /* GPIO_EMC_04 */ (GPIO_PORT4 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_8 /* GPIO_EMC_01 */ (GPIO_PORT4 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { imxrt_config_gpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { imxrt_config_gpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { imxrt_config_gpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { imxrt_config_gpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { imxrt_config_gpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { imxrt_config_gpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { imxrt_config_gpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { imxrt_config_gpio(PROBE_8); } \ + } while(0) + +# define PROBE(n,s) do {imxrt_gpio_write(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __NUTTX_CONFIG_NXP_FMURT1062_V1_INCLUDE_BOARD_H */ diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig new file mode 100644 index 00000000000..0f5a0f6fb26 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig @@ -0,0 +1,245 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="imxrt" +CONFIG_ARCH_CHIP_IMXRT=y +CONFIG_ARCH_CHIP_MIMXRT1062DVL6A=y +CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_ITCM=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_LOOPSPERMSEC=104926 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x001d +CONFIG_CDCACM_PRODUCTSTR="PX4 FMURT1062 v1.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_BLOCKS=y +CONFIG_FS_PROCFS_EXCLUDE_MOUNT=y +CONFIG_FS_PROCFS_EXCLUDE_MOUNTS=y +CONFIG_FS_PROCFS_EXCLUDE_PARTITIONS=y +CONFIG_FS_PROCFS_EXCLUDE_USAGE=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_IMXRT_DTCM_HEAP=y +CONFIG_IMXRT_GPIO1_0_15_IRQ=y +CONFIG_IMXRT_GPIO1_16_31_IRQ=y +CONFIG_IMXRT_GPIO2_0_15_IRQ=y +CONFIG_IMXRT_GPIO2_16_31_IRQ=y +CONFIG_IMXRT_GPIO3_0_15_IRQ=y +CONFIG_IMXRT_GPIO3_16_31_IRQ=y +CONFIG_IMXRT_GPIO4_0_15_IRQ=y +CONFIG_IMXRT_GPIO4_16_31_IRQ=y +CONFIG_IMXRT_GPIO5_0_15_IRQ=y +CONFIG_IMXRT_GPIO5_16_31_IRQ=y +CONFIG_IMXRT_GPIO6_0_15_IRQ=y +CONFIG_IMXRT_GPIO6_16_31_IRQ=y +CONFIG_IMXRT_GPIO7_0_15_IRQ=y +CONFIG_IMXRT_GPIO7_16_31_IRQ=y +CONFIG_IMXRT_GPIO8_0_15_IRQ=y +CONFIG_IMXRT_GPIO8_16_31_IRQ=y +CONFIG_IMXRT_GPIO9_0_15_IRQ=y +CONFIG_IMXRT_GPIO9_16_31_IRQ=y +CONFIG_IMXRT_GPIO_IRQ=y +CONFIG_IMXRT_LPI2C1=y +CONFIG_IMXRT_LPI2C2=y +CONFIG_IMXRT_LPI2C3=y +CONFIG_IMXRT_LPI2C_DYNTIMEO=y +CONFIG_IMXRT_LPI2C_DYNTIMEO_STARTSTOP=10 +CONFIG_IMXRT_LPSPI1=y +CONFIG_IMXRT_LPSPI2=y +CONFIG_IMXRT_LPSPI3=y +CONFIG_IMXRT_LPSPI4=y +CONFIG_IMXRT_LPUART2=y +CONFIG_IMXRT_LPUART3=y +CONFIG_IMXRT_LPUART4=y +CONFIG_IMXRT_LPUART5=y +CONFIG_IMXRT_LPUART6=y +CONFIG_IMXRT_LPUART7=y +CONFIG_IMXRT_LPUART8=y +CONFIG_IMXRT_LPUART_INVERT=y +CONFIG_IMXRT_RTC_MAGIC_REG=1 +CONFIG_IMXRT_SNVS_LPSRTC=y +CONFIG_IMXRT_USDHC1=y +CONFIG_IMXRT_USDHC1_INVERT_CD=y +CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LPUART2_BAUD=57600 +CONFIG_LPUART2_RXBUFSIZE=600 +CONFIG_LPUART2_TXBUFSIZE=1500 +CONFIG_LPUART3_BAUD=57600 +CONFIG_LPUART3_IFLOWCONTROL=y +CONFIG_LPUART3_OFLOWCONTROL=y +CONFIG_LPUART3_RXBUFSIZE=600 +CONFIG_LPUART3_TXBUFSIZE=3000 +CONFIG_LPUART4_BAUD=57600 +CONFIG_LPUART4_IFLOWCONTROL=y +CONFIG_LPUART4_OFLOWCONTROL=y +CONFIG_LPUART4_RXBUFSIZE=600 +CONFIG_LPUART4_TXBUFSIZE=1500 +CONFIG_LPUART5_BAUD=57600 +CONFIG_LPUART5_RXBUFSIZE=600 +CONFIG_LPUART5_TXBUFSIZE=1500 +CONFIG_LPUART6_BAUD=57600 +CONFIG_LPUART6_RXBUFSIZE=600 +CONFIG_LPUART6_TXBUFSIZE=1500 +CONFIG_LPUART7_BAUD=57600 +CONFIG_LPUART7_RXBUFSIZE=120 +CONFIG_LPUART7_SERIAL_CONSOLE=y +CONFIG_LPUART7_TXBUFSIZE=1500 +CONFIG_LPUART8_BAUD=57600 +CONFIG_LPUART8_RXBUFSIZE=600 +CONFIG_LPUART8_TXBUFSIZE=1500 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NFILE_DESCRIPTORS=20 +CONFIG_NFILE_STREAMS=8 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_PSSTACKUSAGE=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_NXFONTS_DISABLE_16BPP=y +CONFIG_NXFONTS_DISABLE_1BPP=y +CONFIG_NXFONTS_DISABLE_24BPP=y +CONFIG_NXFONTS_DISABLE_2BPP=y +CONFIG_NXFONTS_DISABLE_32BPP=y +CONFIG_NXFONTS_DISABLE_4BPP=y +CONFIG_NXFONTS_DISABLE_8BPP=y +CONFIG_PIPES=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAM_SIZE=1048576 +CONFIG_RAM_START=0x20200000 +CONFIG_RAW_BINARY=y +CONFIG_RTC=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDIO_BLOCKSETUP=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TIME_EXTENDED=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/scripts/script.ld b/boards/nxp/fmurt1062-v1/nuttx-config/scripts/script.ld new file mode 100644 index 00000000000..613e943e98b --- /dev/null +++ b/boards/nxp/fmurt1062-v1/nuttx-config/scripts/script.ld @@ -0,0 +1,178 @@ +/**************************************************************************** + * configs/nxp_fmurt1062-v1/scripts/flash.ld + * + * Copyright (C) 2018 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. + * + ****************************************************************************/ + +/* The FMURT1062 has 8MiB of QSPI FLASH beginning at address, + * 0x0060:0000, Upto 512Kb of DTCM RAM beginning at 0x2000:0000, and 1MiB OCRAM + * beginning at 0x2020:0000. Neither DTCM or SDRAM are used in this + * configuratin. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x60000000, LENGTH = 8M + sram (rwx) : ORIGIN = 0x20200000, LENGTH = 512M + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 128K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(flash_config) +EXTERN(image_vector_table) +EXTERN(boot_data) + +ENTRY(_stext) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): + { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + + .ARM.exidx : + { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/nxp/fmurt1062-v1/src/CMakeLists.txt b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt new file mode 100644 index 00000000000..53afd8fbe49 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# Copyright (c) 2016, 2019 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_library(drivers_board + autoleds.c + automount.c + can.c + init.c + led.c + sdhc.c + spi.cpp + timer_config.c + usb.c + manifest.c + imxrt_flexspi_nor_boot.c + imxrt_flexspi_nor_flash.c +) +add_dependencies(drivers_board arch_board_hw_info) + +target_link_libraries(drivers_board + PRIVATE + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) \ No newline at end of file diff --git a/boards/nxp/fmurt1062-v1/src/autoleds.c b/boards/nxp/fmurt1062-v1/src/autoleds.c new file mode 100644 index 00000000000..8106e9f7a86 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/autoleds.c @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 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 shall be used during board bring up of Nuttx. + * + * The NXP FMUK66-V3 has a separate Red, Green and Blue LEDs driven by the K66 + * as follows: + * + * LED K66 + * ------ ------------------------------------------------------- + * RED FB_CS0_b/ UART2_CTS_b / ADC0_SE5b / SPI0_SCK / FTM3_CH1/ PTD1 + * GREEN FTM2_FLT0/ CMP0_IN3/ FB_AD6 / I2S0_RX_BCLK/ FTM3_CH5/ ADC1_SE5b/ PTC9 + * BLUE CMP0_IN2/ FB_AD7 / I2S0_MCLK/ FTM3_CH4/ ADC1_SE4b/ PTC8 + * + * If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the NXP fmurt1062-v1. 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 K66 is in sleep mode (Optional, not used) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#ifdef CONFIG_ARCH_LEDS +__BEGIN_DECLS +extern void led_init(void); +__END_DECLS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +bool nuttx_owns_leds = true; +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ +void phy_set_led(int l, bool s) +{ + +} +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/nxp/fmurt1062-v1/src/automount.c b/boards/nxp/fmurt1062-v1/src/automount.c new file mode 100644 index 00000000000..f69f7fc8f5f --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/automount.c @@ -0,0 +1,304 @@ +/************************************************************************************ + * + * Copyright (C) 2016-2018 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 + +#include "board_config.h" +#ifdef HAVE_AUTOMOUNTER + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This structure represents the changeable state of the automounter */ + +struct fmuk66_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 fmuk66_automount_config_s { + /* This must be first thing in structure so that we can simply cast from struct + * automount_lower_s to struct fmuk66_automount_config_s + */ + + struct automount_lower_s lower; /* Publicly visible part */ + FAR struct fmuk66_automount_state_s *state; /* Changeable state */ +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +static int fmuk66_attach(FAR const struct automount_lower_s *lower, automount_handler_t isr, FAR void *arg); +static void fmuk66_enable(FAR const struct automount_lower_s *lower, bool enable); +static bool fmuk66_inserted(FAR const struct automount_lower_s *lower); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +static struct fmuk66_automount_state_s g_sdhc_state; +static const struct fmuk66_automount_config_s g_sdhc_config = { + .lower = + { + .fstype = CONFIG_FMUK66_SDHC_AUTOMOUNT_FSTYPE, + .blockdev = CONFIG_FMUK66_SDHC_AUTOMOUNT_BLKDEV, + .mountpoint = CONFIG_FMUK66_SDHC_AUTOMOUNT_MOUNTPOINT, + .ddelay = MSEC2TICK(CONFIG_FMUK66_SDHC_AUTOMOUNT_DDELAY), + .udelay = MSEC2TICK(CONFIG_FMUK66_SDHC_AUTOMOUNT_UDELAY), + .attach = fmuk66_attach, + .enable = fmuk66_enable, + .inserted = fmuk66_inserted + }, + .state = &g_sdhc_state +}; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: fmuk66_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 fmuk66_attach(FAR const struct automount_lower_s *lower, automount_handler_t isr, FAR void *arg) +{ + FAR const struct fmuk66_automount_config_s *config; + FAR struct fmuk66_automount_state_s *state; + + /* Recover references to our structure */ + + config = (FAR struct fmuk66_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: fmuk66_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 fmuk66_enable(FAR const struct automount_lower_s *lower, bool enable) +{ + FAR const struct fmuk66_automount_config_s *config; + FAR struct fmuk66_automount_state_s *state; + irqstate_t flags; + + /* Recover references to our structure */ + + config = (FAR struct fmuk66_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 = fmuk66_cardinserted(); + (void)state->handler(&config->lower, state->arg, inserted); + } + + state->pending = false; + } + + leave_critical_section(flags); +} + +/************************************************************************************ + * Name: fmuk66_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 fmuk66_inserted(FAR const struct automount_lower_s *lower) +{ + return fmuk66_cardinserted(); +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: fmuk66_automount_initialize + * + * Description: + * Configure auto-mounters for each enable and so configured SDHC + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ************************************************************************************/ + +void fmuk66_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: fmuk66_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 fmuk66_automount_event(bool inserted) +{ + FAR const struct fmuk66_automount_config_s *config = &g_sdhc_config; + FAR struct fmuk66_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/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h new file mode 100644 index 00000000000..a37991c5687 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/board_config.h @@ -0,0 +1,682 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019 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 + * + * NXP fmukrt1062-v1 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* PX4IO connection configuration */ + +#if 0 // There is no PX4IO Support on first out +// This requires serial DMA driver +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS6" +#define PX4IO_SERIAL_TX_GPIO GPIO_LPUART8_TX_2 +#define PX4IO_SERIAL_RX_GPIO GPIO_LPUART8_RX_2 +#define PX4IO_SERIAL_BASE IMXRT_LPUART8_BASE +#define PX4IO_SERIAL_VECTOR IMXRT_IRQ_LPUART8 +#define PX4IO_SERIAL_TX_DMAMAP +#define PX4IO_SERIAL_RX_DMAMAP +#define PX4IO_SERIAL_RCC_REG +#define PX4IO_SERIAL_RCC_EN +#define PX4IO_SERIAL_CLOCK +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ +#endif + +/* Configuration ************************************************************************************/ + +/* FMURT1062 GPIOs ***********************************************************************************/ +/* LEDs */ +/* An RGB LED is connected through GPIO as shown below: + */ +#define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define GPIO_nLED_RED /* GPIO_B0_00 QTIMER1_TIMER0 GPIO2_IO0 */ (GPIO_PORT2 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) +#define GPIO_nLED_GREEN /* GPIO_B0_01 QTIMER1_TIMER1 GPIO2_IO1 */ (GPIO_PORT2 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) +#define GPIO_nLED_BLUE /* GPIO_B1_08 QTIMER1_TIMER3 GPIO2_IO24 */ (GPIO_PORT2 | GPIO_PIN24 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* SENSORS are on SPI1, 3 + * MEMORY is on bus SPI2 + * MS5611 is on bus SPI4 + */ + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_MEMORY 2 +#define PX4_SPI_BUS_BARO 3 +#define PX4_SPI_BUS_EXTERNAL1 4 + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) + +/* Define the Chip Selects, Data Ready and Control signals per SPI bus */ + +#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST) +#define OUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + +/* SPI 1 CS */ +#define GPIO_SPI1_CS1_ICM20689 /* GPIO_EMC_40 GPIO3_IO26 */ (GPIO_PORT3 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) +// GPIO_SPI1_CS2_ICM20602 is not wired from CPU +#define GPIO_SPI1_CS3_BMI055_GYRO /* GPIO_B1_10 GPIO2_IO26 */ (GPIO_PORT2 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) +#define GPIO_SPI1_CS4_BMI055_ACCEL /* GPIO_B1_15 GPIO2_IO31 */ (GPIO_PORT2 | GPIO_PIN31 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) + +#define SPI1_CS5_AUX_MEM /* GPIO_SD_B1_00 GPIO3_IO00 */ (GPIO_PORT3 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) + +/* Define the SPI1 Data Ready interrupts */ + +#define DRDY_IOMUX (IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ) + +// todo add IRQ-ness! + +#define GPIO_SPI1_DRDY1_ICM20689 /* GPIO_EMC_15 GPIO4_IO15 */ (GPIO_PORT4 | GPIO_PIN15 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI1_DRDY2_BMI055_GYRO /* GPIO_EMC_16 GPIO4_IO16 */ (GPIO_PORT4 | GPIO_PIN16 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI1_DRDY3_BMI055_ACCEL /* GPIO_EMC_37 GPIO3_IO23 */ (GPIO_PORT3 | GPIO_PIN23 | GPIO_INPUT | DRDY_IOMUX) +#define SPI1_DRDY4_ICM20602 /* GPIO_EMC_12 GPIO4_IO12 N.B . The ICM20602 CS is not wired */ (GPIO_PORT4 | GPIO_PIN12 | GPIO_INPUT | DRDY_IOMUX) + +/* SPI1 off */ + +#define _GPIO_LPSPI1_SCK /* GPIO_EMC_27 GPIO4_IO27 */ (GPIO_PORT4 | GPIO_PIN27 | CS_IOMUX) +#define _GPIO_LPSPI1_MISO /* GPIO_EMC_29 GPIO4_IO29 */ (GPIO_PORT4 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI1_MOSI /* GPIO_EMC_28 GPIO4_IO28 */ (GPIO_PORT4 | GPIO_PIN28 | CS_IOMUX) + +#define GPIO_SPI1_SCK_OFF _PIN_OFF(_GPIO_LPSPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(_GPIO_LPSPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(_GPIO_LPSPI1_MOSI) + +#define GPIO_DRDY_OFF_SPI1_DRDY1_ICM20689 _PIN_OFF(GPIO_SPI1_DRDY1_ICM20689) +#define GPIO_DRDY_OFF_SPI1_DRDY2_BMI055_GYRO _PIN_OFF(GPIO_SPI1_DRDY2_BMI055_GYRO) +#define GPIO_DRDY_OFF_SPI1_DRDY3_BMI055_ACC _PIN_OFF(GPIO_SPI1_DRDY3_BMI055_ACCEL) +#define GPIO_DRDY_OFF_SPI1_DRDY4_ICM20602 _PIN_OFF(GPIO_SPI1_DRDY4_ICM20602) + +/* SPI 2 CS */ + +#define GPIO_SPI2_CS_FRAM /* GPIO_EMC_34 G GPIO3_IO20 */ (GPIO_PORT3 | GPIO_PIN20 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) + +/* SPI 3 CS */ + +#define GPIO_SPI3_CS1_MS5611 /* GPIO_EMC_14 GPIO4_IO14 */ (GPIO_PORT4 | GPIO_PIN14 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) + +#define _GPIO_LPSPI3_SCK /* GPIO_AD_B1_15 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN31 | CS_IOMUX) +#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN30 | CS_IOMUX) + +#define GPIO_SPI3_SCK_OFF _PIN_OFF(_GPIO_LPSPI3_SCK) +#define GPIO_SPI3_MISO_OFF _PIN_OFF(_GPIO_LPSPI3_MISO) +#define GPIO_SPI3_MOSI_OFF _PIN_OFF(_GPIO_LPSPI3_MOSI) + +/* SPI 4 CS */ + +#define SPI4_CS1_EXTERNAL1 /* GPIO_EMC_07 GPIO4_IO07 */ (GPIO_PORT4 | GPIO_PIN7 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) +#define SPI4_CS2_EXTERNAL1 /* GPIO_B1_14 GPIO2_IO30 */ (GPIO_PORT2 | GPIO_PIN30 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) +#define SPI4_CS3_EXTERNAL1 /* GPIO_EMC_11 GPIO4_IO011 */ (GPIO_PORT4 | GPIO_PIN11 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX) + +/* Define the SPI4 Data Ready and Control signals */ + +#define GPIO_SPI4_DRDY7_EXTERNAL1 /* GPIO_EMC_35 GPIO3_IO21*/ (GPIO_PORT3 | GPIO_PIN21 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_nSPI4_RESET_EXTERNAL1 /* GPIO_B1_00 GPIO2_IO16 */ (GPIO_PORT2 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX) +#define GPIO_SPI4_SYNC_EXTERNAL1 /* GPIO_EMC_05 GPIO4_IO5 */(GPIO_PORT4 | GPIO_PIN5 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX) + +#define GPIO_DRDY_OFF_SPI4_DRDY7_EXTERNAL1 _PIN_OFF(GPIO_SPI4_DRDY7_EXTERNAL1) +#define GPIO_nSPI4_RESET_EXTERNAL1_OFF _PIN_OFF(GPIO_nSPI4_RESET_EXTERNAL1) +#define GPIO_SPI4_SYNC_EXTERNAL1_OFF _PIN_OFF(GPIO_SPI4_SYNC_EXTERNAL1) + + +/* v BEGIN Legacy SPI defines TODO: fix this with enumeration */ + +#define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_MEMORY +#define PX4_SPIDEV_BMA 0 +#define PX4_SPIDEV_BMI 0 + +/* ^ END Legacy SPI defines TODO: fix this with enumeration */ + +#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) +#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,1) +#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,2) +#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,3) + +#define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI1_CS1_ICM20689, GPIO_SPI1_CS3_BMI055_GYRO, GPIO_SPI1_CS4_BMI055_ACCEL, SPI1_CS5_AUX_MEM} +#define PX4_SENSORS_BUS_FIRST_CS PX4_SPIDEV_ICM_20689 +#define PX4_SENSORS_BUS_LAST_CS PX4_SPIDEV_AUX_MEM + +#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI2_CS_FRAM} +#define PX4_MEMORY_BUS_FIRST_CS PX4_SPIDEV_MEMORY +#define PX4_MEMORY_BUS_LAST_CS PX4_SPIDEV_MEMORY + +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO,0) +#define PX4_BARO_BUS_CS_GPIO {GPIO_SPI3_CS1_MS5611} +#define PX4_BARO_BUS_FIRST_CS PX4_SPIDEV_BARO +#define PX4_BARO_BUS_LAST_CS PX4_SPIDEV_BARO + +#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,0) +#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,1) +#define PX4_SPIDEV_EXTERNAL1_3 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,2) +#define PX4_EXTERNAL1_BUS_CS_GPIO {SPI4_CS1_EXTERNAL1, SPI4_CS2_EXTERNAL1, SPI4_CS3_EXTERNAL1} +#define PX4_EXTERNAL1_BUS_FIRST_CS PX4_SPIDEV_EXTERNAL1_1 +#define PX4_EXTERNAL1_BUS_LAST_CS PX4_SPIDEV_EXTERNAL1_3 + +/* I2C busses */ + +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_EXPANSION1 2 +#define PX4_I2C_BUS_ONBOARD 3 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +#define BOARD_NUMBER_I2C_BUSES 3 +#define BOARD_I2C_BUS_CLOCK_INIT {100000, 100000, 100000} + + +#define ADC_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) + +#define ADC1_CH(n) (n) +#define ADC1_GPIO(n, p) (GPIO_PORT1 | GPIO_PIN##p | ADC_IOMUX) // + +/* Define GPIO pins used as ADC N.B. Channel numbers are for reference, */ + +#define PX4_ADC_GPIO \ + /* BATTERY1_VOLTAGE GPIO_AD_B1_11 GPIO1 Pin 27 */ ADC1_GPIO(0, 27), \ + /* BATTERY1_CURRENT GPIO_AD_B0_12 GPIO1 Pin 12 */ ADC1_GPIO(1, 12), \ + /* BATTERY2_VOLTAGE GPIO_AD_B0_13 GPIO1 Pin 13 */ ADC1_GPIO(2, 13), \ + /* BATTERY2_CURRENT GPIO_AD_B0_14 GPIO1 Pin 14 */ ADC1_GPIO(3, 14), \ + /* SPARE_2_CHANNEL GPIO_AD_B0_15 GPIO1 Pin 15 */ ADC1_GPIO(4, 15), \ + /* HW_VER_SENSE GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_GPIO(9, 20), \ + /* SCALED_V5 GPIO_AD_B1_05 GPIO1 Pin 21 */ ADC1_GPIO(10, 21), \ + /* SCALED_VDD_3V3_SENSORS GPIO_AD_B1_06 GPIO1 Pin 22 */ ADC1_GPIO(11, 22), \ + /* HW_REV_SENSE GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_GPIO(13, 24), \ + /* SPARE_1 GPIO_AD_B1_09 GPIO1 Pin 25 */ ADC1_GPIO(14, 25), \ + /* RSSI_IN GPIO_AD_B1_10 GPIO1 Pin 26 */ ADC1_GPIO(15, 26) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY1_VOLTAGE_CHANNEL /* GPIO_AD_B1_11 GPIO1 Pin 27 */ ADC1_CH(0) +#define ADC_BATTERY1_CURRENT_CHANNEL /* GPIO_AD_B0_12 GPIO1 Pin 12 */ ADC1_CH(1) +#define ADC_BATTERY2_VOLTAGE_CHANNEL /* GPIO_AD_B0_13 GPIO1 Pin 13 */ ADC1_CH(2) +#define ADC_BATTERY2_CURRENT_CHANNEL /* GPIO_AD_B0_14 GPIO1 Pin 14 */ ADC1_CH(3) +#define ADC1_SPARE_2_CHANNEL /* GPIO_AD_B0_15 GPIO1 Pin 15 */ ADC1_CH(4) +#define ADC_HW_VER_SENSE_CHANNEL /* GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_CH(9) +#define ADC_SCALED_V5_CHANNEL /* GPIO_AD_B1_05 GPIO1 Pin 21 */ ADC1_CH(10) +#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* GPIO_AD_B1_06 GPIO1 Pin 22 */ ADC1_CH(11) +#define ADC_HW_REV_SENSE_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_CH(13) +#define ADC1_SPARE_1_CHANNEL /* GPIO_AD_B1_09 GPIO1 Pin 25 */ ADC1_CH(14) +#define ADC_RSSI_IN_CHANNEL /* GPIO_AD_B1_10 GPIO1 Pin 26 */ ADC1_CH(15) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC1_SPARE_2_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL) | \ + (1 << ADC_HW_REV_SENSE_CHANNEL) | \ + (1 << ADC1_SPARE_1_CHANNEL)) + +/* Define Battery 1 Voltage Divider and A per V */ + +#define BOARD_BATTERY1_V_DIV (10.1097f) /* measured with the provided PM board */ +#define BOARD_BATTERY1_A_PER_V (15.391030303f) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ + +#define BOARD_HAS_HW_VERSIONING + +#define HW_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MAX | IOMUX_SLEW_FAST) + +#define GPIO_HW_VER_REV_DRIVE /* GPIO_AD_B0_01 GPIO1_IO01 */ (GPIO_PORT1 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | HW_IOMUX) +#define GPIO_HW_REV_SENSE /* GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_GPIO(13, 24) +#define GPIO_HW_VER_SENSE /* GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_GPIO(9, 20) +#define HW_INFO_INIT {'V','5','x', 'x',0} +#define HW_INFO_INIT_VER 2 +#define HW_INFO_INIT_REV 3 +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define SILENT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MAX | IOMUX_SLEW_FAST) +#define GPIO_CAN1_SILENT_S0 /* GPIO_AD_B0_10 GPIO1_IO10 */ (GPIO_PORT1 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX) +#define GPIO_CAN2_SILENT_S1 /* GPIO_EMC_06 GPIO4_IO06 */ (GPIO_PORT4 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX) +#define GPIO_CAN3_SILENT_S2 /* GPIO_EMC_09 GPIO4_IO09 */ (GPIO_PORT4 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX) + +/* HEATER + * PWM in future + */ +#define HEATER_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) +#define GPIO_HEATER_OUTPUT /* GPIO_B1_09 QTIMER2_TIMER3 GPIO2_IO25 */ (GPIO_QTIMER2_TIMER3_1 | HEATER_IOMUX) + +/* PWM Capture + * + * 2 PWM Capture inputs are supported + */ +#define DIRECT_PWM_CAPTURE_CHANNELS 2 +#define CAP_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) +#define PIN_FLEXPWM2_PWMB0 /* P2:7 PWM2 B0 FMU_CAP1 */ (CAP_IOMUX | GPIO_FLEXPWM2_PWMB00_2) +#define PIN_FLEXPWM2_PWMB3 /* P3:3 PWM2 A1 FMU_CAP2 */ (CAP_IOMUX | GPIO_FLEXPWM2_PWMB03_3) + +#define nARMED_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ) +#define nARMED_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + +#define GPIO_nARMED_INIT /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_INPUT | nARMED_INPUT_IOMUX) +#define GPIO_nARMED /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX) + +#define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) + +/* PWM + * + * 8 PWM outputs are configured. + * + * Pins: + * + * FMU_CH1 : GPIO_B0_06 GPIO2 Pin 6 FLEXPWM2_PWMA0 + * FMU_CH2 : GPIO_EMC_08 GPIO4 Pin 8 FLEXPWM2_PWMA1 + * FMU_CH3 : GPIO_EMC_10 GPIO4 Pin 10 FLEXPWM2_PWMA2 + * FMU_CH4 : GPIO_AD_B0_09 GPIO1 Pin 9 FLEXPWM2_PWMA3 + * FMU_CH5 : GPIO_EMC_33 GPIO3 Pin 19 FLEXPWM3_PWMA2 + * FMU_CH6 : GPIO_EMC_30 GPIO4 Pin 30 FLEXPWM3_PWMB0 + * FMU_CH7 : GPIO_EMC_04 GPIO4 Pin 4 FLEXPWM4_PWMA2 + * FMU_CH8 : GPIO_EMC_01 GPIO4 Pin 1 FLEXPWM4_PWMB0 + * + */ +#define PWM_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) +#define PIN_FLEXPWM2_PWMA00 /* P2:6 GPIO_B0_06 PWM2 A0 FMU1 */ (PWM_IOMUX | GPIO_FLEXPWM2_PWMA00_2) +#define PIN_FLEXPWM2_PWMA01 /* P4:8 GPIO_EMC_08 PWM2 A1 FMU2 */ (PWM_IOMUX | GPIO_FLEXPWM2_PWMA01_1) +#define PIN_FLEXPWM2_PWMA02 /* P4:10 GPIO_EMC_10 PWM2 A2 FMU3 */ (PWM_IOMUX | GPIO_FLEXPWM2_PWMA02_1) +#define PIN_FLEXPWM2_PWMA03 /* P1:9 GPIO_AD_B0_09 PWM2 A3 FMU4 */ (PWM_IOMUX | GPIO_FLEXPWM2_PWMA03_2) +#define PIN_FLEXPWM3_PWMA02 /* P3:19 GPIO_EMC_33 PWM3 A2 FMU5 */ (PWM_IOMUX | GPIO_FLEXPWM3_PWMA02_1) +#define PIN_FLEXPWM3_PWMB00 /* P4:30 GPIO_EMC_30 PWM3 B0 FMU6 */ (PWM_IOMUX | GPIO_FLEXPWM3_PWMB00_1) +#define PIN_FLEXPWM4_PWMA02 /* P4:4 GPIO_EMC_04 PWM4 A2 FMU7 */ (PWM_IOMUX | GPIO_FLEXPWM4_PWMA02_2) +#define PIN_FLEXPWM4_PWMB00 /* P4:1 GPIO_EMC_01 PWM4 B0 FMU8 */ (PWM_IOMUX | GPIO_FLEXPWM4_PWMB00_1) + +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +// Input Capture not supported on MVP + +#define DIRECT_INPUT_TIMER_CHANNELS 0 + +#define BOARD_HAS_LED_PWM 1 +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 + +/* UI LEDs are driven by timer 4 the pins have no alternates + * + * nUI_LED_RED GPIO_B0_10 GPIO2_IO10 QTIMER4_TIMER1 + * nUI_LED_GREEN GPIO_B0_11 GPIO2_IO11 QTIMER4_TIMER2 + * nUI_LED_BLUE GPIO_B1_11 GPIO2_IO27 QTIMER4_TIMER3 + */ + + +/* User GPIOs + * + * GPIO- + * Define as GPIO input / GPIO outputs + */ + +#define FMU_INPUT_IOMUX (IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ) + +#define _MK_GPIO_INPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | FMU_INPUT_IOMUX)) + +#define GPIO_GPIO0_INPUT /* P2:6 GPIO_B0_06 PWM2 A0 FMU1 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO1_INPUT /* P4:8 GPIO_EMC_08 PWM2 A1 FMU2 */ (GPIO_PORT4 | GPIO_PIN8 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO2_INPUT /* P4:10 GPIO_EMC_10 PWM2 A2 FMU3 */ (GPIO_PORT4 | GPIO_PIN10 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO3_INPUT /* P1:9 GPIO_AD_B0_09 PWM2 A3 FMU4 */ (GPIO_PORT1 | GPIO_PIN9 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO4_INPUT /* P3:19 GPIO_EMC_33 PWM3 A2 FMU5 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO5_INPUT /* P4:30 GPIO_EMC_30 PWM3 B0 FMU6 */ (GPIO_PORT4 | GPIO_PIN30 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO6_INPUT /* P4:4 GPIO_EMC_04 PWM4 A2 FMU7 */ (GPIO_PORT4 | GPIO_PIN4 | GPIO_INPUT | FMU_INPUT_IOMUX) +#define GPIO_GPIO7_INPUT /* P4:1 GPIO_EMC_01 PWM4 B0 FMU8 */ (GPIO_PORT4 | GPIO_PIN1 | GPIO_INPUT | FMU_INPUT_IOMUX) + +#define FMU_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + +#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX)) + +#define GPIO_GPIO0_OUTPUT /* P2:6 GPIO_B0_06 PWM2 A0 FMU1 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO1_OUTPUT /* P4:8 GPIO_EMC_08 PWM2 A1 FMU2 */ (GPIO_PORT4 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO2_OUTPUT /* P4:10 GPIO_EMC_10 PWM2 A2 FMU3 */ (GPIO_PORT4 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO3_OUTPUT /* P1:9 GPIO_AD_B0_09 PWM2 A3 FMU4 */ (GPIO_PORT1 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO4_OUTPUT /* P3:19 GPIO_EMC_33 PWM3 A2 FMU5 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO5_OUTPUT /* P4:30 GPIO_EMC_30 PWM3 B0 FMU6 */ (GPIO_PORT4 | GPIO_PIN30 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO6_OUTPUT /* P4:4 GPIO_EMC_04 PWM4 A2 FMU7 */ (GPIO_PORT4 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) +#define GPIO_GPIO7_OUTPUT /* P4:1 GPIO_EMC_01 PWM4 B0 FMU8 */ (GPIO_PORT4 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | FMU_OUTPUT_IOMUX) + +/* Power supply control and monitoring GPIOs */ + +#define GENERAL_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ) +#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + +#define GPIO_nPOWER_IN_A /* GPIO_B0_12 GPIO2_IO12 */ (GPIO_PORT2 | GPIO_PIN12 | GPIO_INPUT | GENERAL_INPUT_IOMUX) +#define GPIO_nPOWER_IN_B /* GPIO_B0_13 GPIO2_IO13 */ (GPIO_PORT2 | GPIO_PIN13 | GPIO_INPUT | GENERAL_INPUT_IOMUX) +#define GPIO_nPOWER_IN_C /* GPIO_B0_14 GPIO2_IO14 */ (GPIO_PORT2 | GPIO_PIN14 | GPIO_INPUT | GENERAL_INPUT_IOMUX) + +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ +#define BOARD_NUMBER_BRICKS 2 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ + +#define OC_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) + +#define GPIO_nVDD_5V_PERIPH_EN /* GPIO_B1_04 GPIO2_IO20 */ (GPIO_PORT2 | GPIO_PIN20 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_nVDD_5V_PERIPH_OC /* GPIO_B1_03 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_INPUT | OC_INPUT_IOMUX) +#define GPIO_nVDD_5V_HIPOWER_EN /* GPIO_B1_02 GPIO2_IO18 */ (GPIO_PORT2 | GPIO_PIN18 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_nVDD_5V_HIPOWER_OC /* GPIO_B1_01 GPIO2_IO17 */ (GPIO_PORT2 | GPIO_PIN17 | GPIO_INPUT | OC_INPUT_IOMUX) +#define GPIO_VDD_3V3_SENSORS_EN /* GPIO_EMC_41 GPIO3_IO27 */ (GPIO_PORT3 | GPIO_PIN27 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* GPIO_AD_B0_00 GPIO1_IO00 */ (GPIO_PORT1 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_5V_RC_EN /* GPIO_AD_B0_08 GPIO1_IO08 */ (GPIO_PORT1 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_5V_WIFI_EN /* PMIC_STBY_REQ GPIO5_IO02 */ (GPIO_PORT5 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SD_CARD_EN /* GPIO_EMC_13 GPIO4_IO13 */ (GPIO_PORT4 | GPIO_PIN13 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO |GENERAL_OUTPUT_IOMUX) + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_PERIPH_EN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_HIPOWER_EN, !(on_true)) +#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true)) +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) +#define VDD_5V_RC_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_RC_EN, (on_true)) +#define VDD_5V_WIFI_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_WIFI_EN, (on_true)) +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 2 /* GPT 2 */ +#define TONE_ALARM_CHANNEL 3 /* GPIO_AD_B1_07 GPT2_COMPARE3 */ + +#define GPIO_BUZZER_1 /* GPIO_AD_B1_07 GPIO1_IO23 */ (GPIO_PORT1 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM (GPIO_GPT2_COMPARE3_2 | GENERAL_OUTPUT_IOMUX) + +/* USB OTG FS + * + * VBUS_VALID is detected in USB_ANALOG_USB1_VBUS_DETECT_STAT + */ + +/* High-resolution timer */ +#define HRT_TIMER 1 /* use GPT1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* GPIO_B1_06 GPT1_CAPTURE2 */ 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN /* GPIO_B1_06 GPT1_CAPTURE2 */ (GPIO_GPT1_CAPTURE2_2 | GENERAL_INPUT_IOMUX) + +#define RC_SERIAL_PORT "/dev/ttyS5" + +/* PWM input driver. Use FMU AUX5 pins attached to GPIO_EMC_33 GPIO3_IO19 FLEXPWM3_PWMA2 */ + +#define PWMIN_TIMER /* FLEXPWM3_PWMA2 */ 3 +#define PWMIN_TIMER_CHANNEL /* FLEXPWM3_PWMA2 */ 2 +#define GPIO_PWM_IN /* GPIO_EMC_33 GPIO3_IO19 */ (GPIO_FLEXPWM3_PWMA02_1 | GENERAL_INPUT_IOMUX) + +/* Shared pins Both FMU and PX4IO control/monitor + * FMU Initializes these pins to passive input until it is known + * if we have and PX4IO on board + */ + +#define GPIO_RSSI_IN /* GPIO_AD_B1_10 GPIO1_IO26 */ (GPIO_PORT1 | GPIO_PIN26 | GPIO_INPUT | ADC_IOMUX) +#define GPIO_RSSI_IN_INIT /* GPIO_AD_B1_10 GPIO1_IO26 */ 0 /* Using 0 will Leave as ADC RSSI_IN */ + +/* Safety Switch is HW version dependent on having an PX4IO + * So we init to a benign state with the _INIT definition + * and provide the the non _INIT one for the driver to make a run time + * decision to use it. + */ +#define SAFETY_INIT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) +#define SAFETY_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define SAFETY_SW_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ) + +#define GPIO_nSAFETY_SWITCH_LED_OUT_INIT /* GPIO_B0_15 GPIO2_IO15 */ (GPIO_PORT2 | GPIO_PIN15 | GPIO_INPUT | SAFETY_INIT_IOMUX) +#define GPIO_nSAFETY_SWITCH_LED_OUT /* GPIO_B0_15 GPIO2_IO15 */ (GPIO_PORT2 | GPIO_PIN15 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | SAFETY_IOMUX) + +/* Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_LED_SAFETY GPIO_nSAFETY_SWITCH_LED_OUT +#define GPIO_SAFETY_SWITCH_IN /* GPIO_AD_B1_12 GPIO1_IO28 */ (GPIO_PORT1 | GPIO_PIN28 | GPIO_INPUT | SAFETY_SW_IOMUX) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* + * FMUv5 has a separate RC_IN + * + * GPIO PPM_IN on GPIO_EMC_23 GPIO4 Pin 23 GPT1_CAPTURE2 + * Inversion is possible in the UART and can drive GPIO PPM_IN as an output + */ + +#define GPIO_PPM_IN_AS_OUT /* GPIO_B1_06 GPIO2_IO23 GPT1_CAPTURE2 GPT1_CAPTURE2 */ (GPIO_PORT2 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX) + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* 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_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) +#define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0) + +/* FMUv5 never powers odd the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +/* This board provides a DMA pool and APIs */ + +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +/* The list of GPIO that will be initialized */ + +#define PX4_GPIO_PWM_INIT_LIST { \ + GPIO_GPIO7_INPUT, \ + GPIO_GPIO6_INPUT, \ + GPIO_GPIO5_INPUT, \ + GPIO_GPIO4_INPUT, \ + GPIO_GPIO3_INPUT, \ + GPIO_GPIO2_INPUT, \ + GPIO_GPIO1_INPUT, \ + GPIO_GPIO0_INPUT, \ + } + +#define PX4_GPIO_INIT_LIST { \ + GPIO_nARMED_INIT, \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_FLEXCAN1_TX, \ + GPIO_FLEXCAN1_RX, \ + GPIO_FLEXCAN2_TX, \ + GPIO_FLEXCAN2_RX, \ + GPIO_FLEXCAN3_TX, \ + GPIO_FLEXCAN3_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN2_SILENT_S1, \ + GPIO_CAN3_SILENT_S2, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_nPOWER_IN_C, \ + GPIO_nVDD_5V_PERIPH_EN, \ + GPIO_nVDD_5V_PERIPH_OC, \ + GPIO_nVDD_5V_HIPOWER_EN, \ + GPIO_nVDD_5V_HIPOWER_OC, \ + GPIO_VDD_3V3_SENSORS_EN, \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_5V_RC_EN, \ + GPIO_VDD_5V_WIFI_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_RSSI_IN_INIT, \ + GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \ + GPIO_nSPI4_RESET_EXTERNAL1, \ + GPIO_SPI4_SYNC_EXTERNAL1, \ + GPIO_SAFETY_SWITCH_IN \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void); + +/**************************************************************************************************** + * Name: imxrt_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void imxrt_spidev_initialize(void); + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +extern int imxrt1062_spi_bus_initialize(void); + +/************************************************************************************ + * Name: imxrt_usb_initialize + * + * Description: + * Called to configure USB. + * + ************************************************************************************/ + +extern int imxrt_usb_initialize(void); + +void board_spi_reset(int ms); + +extern void imxrt_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +extern void fmurt1062_timer_initialize(void); +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/fmurt1062-v1/src/can.c b/boards/nxp/fmurt1062-v1/src/can.c new file mode 100644 index 00000000000..dda8e78caed --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/can.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018 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 can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include +#include "up_arch.h" + +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_IMXRT_FLEXCAN1) && defined(CONFIG_IMXRT_FLEXCAN2) \ + && defined(CONFIG_IMXRT_FLEXCAN3) +# warning "CAN1 and CAN2 and CAN2 are enabled. Assuming only CAN1." +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN1 +# 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 imxrt_caninitialize() to get an instance of the CAN interface */ + + can = imxrt_can_initialize(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/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.c b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.c new file mode 100644 index 00000000000..6ef051e9e01 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.c @@ -0,0 +1,64 @@ +/**************************************************************************** + * config/imxrt1060-evk/src/imxrt_flexspi_nor_boot.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Ivan Ucherdzhiev + * + * 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 "imxrt_flexspi_nor_boot.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +__attribute__((section(".boot_hdr.ivt"))) +const struct ivt_s image_vector_table = { + IVT_HEADER, /* IVT Header */ + 0x60002000, /* Image Entry Function */ + IVT_RSVD, /* Reserved = 0 */ + (uint32_t)DCD_ADDRESS, /* Address where DCD information is stored */ + (uint32_t)BOOT_DATA_ADDRESS, /* Address where BOOT Data Structure is stored */ + (uint32_t) &image_vector_table, /* Pointer to IVT Self (absolute address */ + (uint32_t)CSF_ADDRESS, /* Address where CSF file is stored */ + IVT_RSVD /* Reserved = 0 */ +}; + +__attribute__((section(".boot_hdr.boot_data"))) +const struct boot_data_s boot_data = { + FLASH_BASE, /* boot start location */ + (FLASH_END - FLASH_BASE), /* size */ + PLUGIN_FLAG, /* Plugin flag*/ + 0xFFFFFFFF /* empty - extra data word */ +}; diff --git a/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.h b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.h new file mode 100644 index 00000000000..ebc39e8d735 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_boot.h @@ -0,0 +1,147 @@ +/**************************************************************************** + * config/imxrt1060-evk/src/imxrt_flexspi_nor_boot.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Ivan Ucherdzhiev + * 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 __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H +#define __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* IVT Data */ + +#define IVT_MAJOR_VERSION 0x4 +#define IVT_MAJOR_VERSION_SHIFT 0x4 +#define IVT_MAJOR_VERSION_MASK 0xf +#define IVT_MINOR_VERSION 0x1 +#define IVT_MINOR_VERSION_SHIFT 0x0 +#define IVT_MINOR_VERSION_MASK 0xf + +#define IVT_VERSION(major, minor) \ + ((((major) & IVT_MAJOR_VERSION_MASK) << IVT_MAJOR_VERSION_SHIFT) | \ + (((minor) & IVT_MINOR_VERSION_MASK) << IVT_MINOR_VERSION_SHIFT)) + +#define IVT_TAG_HEADER (0xd1) /* Image Vector Table */ +#define IVT_SIZE 0x2000 +#define IVT_PAR IVT_VERSION(IVT_MAJOR_VERSION, IVT_MINOR_VERSION) + +#define IVT_HEADER (IVT_TAG_HEADER | (IVT_SIZE << 8) | (IVT_PAR << 24)) +#define IVT_RSVD (uint32_t)(0x00000000) + +/* DCD Data */ + +#define DCD_TAG_HEADER (0xd2) +#define DCD_TAG_HEADER_SHIFT (24) +#define DCD_VERSION (0x40) +#define DCD_ARRAY_SIZE 1 + +#define FLASH_BASE 0x60000000 +#define FLASH_END 0x7f7fffff +#define SCLK 1 + +#define DCD_ADDRESS 0 +#define BOOT_DATA_ADDRESS &boot_data +#define CSF_ADDRESS 0 +#define PLUGIN_FLAG (uint32_t)0 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* IVT Data */ + +struct ivt_s { + /* Header with tag #HAB_TAG_IVT, length and HAB version fields + * (see data) + */ + + uint32_t hdr; + + /* Absolute address of the first instruction to execute from the + * image + */ + + uint32_t entry; + + /* Reserved in this version of HAB: should be NULL. */ + + uint32_t reserved1; + + /* Absolute address of the image DCD: may be NULL. */ + + uint32_t dcd; + + /* Absolute address of the Boot Data: may be NULL, but not interpreted + * any further by HAB + */ + + uint32_t boot_data; + + /* Absolute address of the IVT.*/ + + uint32_t self; + + /* Absolute address of the image CSF.*/ + + uint32_t csf; + + /* Reserved in this version of HAB: should be zero. */ + + uint32_t reserved2; +}; + +/* Boot Data */ + +struct boot_data_s { + uint32_t start; /* boot start location */ + uint32_t size; /* size */ + uint32_t plugin; /* plugin flag - 1 if downloaded application is plugin */ + uint32_t placeholder; /* placehoder to make even 0x10 size */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const struct boot_data_s boot_data; + +#endif /* __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H */ diff --git a/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.c b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.c new file mode 100644 index 00000000000..44db1286286 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.c @@ -0,0 +1,198 @@ +/**************************************************************************** + * config/imxrt1060-evk/src/imxrt_flexspi_nor_flash.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Ivan Ucherdzhiev + * 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 "imxrt_flexspi_nor_flash.h" + +/******************************************************************************* + * Public Data + ******************************************************************************/ + +#if defined (CONFIG_NXP_FMURT1062_V3_HYPER_FLASH) +__attribute__((section(".boot_hdr.conf"))) +const struct flexspi_nor_config_s flash_config = { + .mem_config = + { + .tag = FLEXSPI_CFG_BLK_TAG, + .version = FLEXSPI_CFG_BLK_VERSION, + .read_sample_clksrc = FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD, + .cs_hold_time = 3u, + .cs_setup_time = 3u, + .column_address_width = 3u, + + /* Enable DDR mode, Word addassable, Safe configuration, Differential clock */ + + .controller_misc_option = (1u << FLEXSPIMISC_OFFSET_DDR_MODE_EN) | + (1u << FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN) | + (1u << FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN) | + (1u << FLEXSPIMISC_OFFSET_DIFFCLKEN), + .sflash_pad_type = SERIAL_FLASH_8PADS, + .serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_133MHz, + .sflash_a1size = 64u * 1024u * 1024u, + .data_valid_time = {16u, 16u}, + .lookup_table = + { + /* Read LUTs */ + + FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xA0, RADDR_DDR, FLEXSPI_8PAD, 0x18), + FLEXSPI_LUT_SEQ(CADDR_DDR, FLEXSPI_8PAD, 0x10, DUMMY_DDR, FLEXSPI_8PAD, 0x06), + FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP, FLEXSPI_1PAD, 0x0), + }, + }, + .page_size = 512u, + .sector_size = 256u * 1024u, + .blocksize = 256u * 1024u, + .is_uniform_blocksize = 1, +}; + +#elif defined (CONFIG_NXP_FMURT1062_V3_QSPI_FLASH) +__attribute__((section(".boot_hdr.conf"))) +const struct flexspi_nor_config_s flash_config = { + .mem_config = + { + .tag = FLEXSPI_CFG_BLK_TAG, + .version = FLEXSPI_CFG_BLK_VERSION, + .read_sample_clksrc = FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD, + .cs_hold_time = 3u, + .cs_setup_time = 3u, + .column_address_width = 0u, + .device_type = FLEXSPI_DEVICE_TYPE_SERIAL_NOR, + .sflash_pad_type = SERIAL_FLASH_4PADS, + .serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_60MHz, + .sflash_a1size = 8u * 1024u * 1024u, + .data_valid_time = {16u, 16u}, + .lookup_table = + { + /* LUTs */ + /* 0 Fast read Quad IO DTR Mode Operation in SPI Mode (normal read)*/ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xED, RADDR_DDR, FLEXSPI_4PAD, 0x18), + FLEXSPI_LUT_SEQ(DUMMY_DDR, FLEXSPI_4PAD, 0x0C, READ_DDR, FLEXSPI_4PAD, 0x08), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + /* 1 Read Status */ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x05, READ_SDR, FLEXSPI_1PAD, 0x1), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + /* 2 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 3 */ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x06, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + /* 4 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 5 Erase Sector */ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xD7, RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + /* 6 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 7 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 8 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 9 Page Program */ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x02, RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(WRITE_SDR, FLEXSPI_1PAD, 0x8, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + /* 10 */ + + 0x00000000, + 0x00000000, + 0x00000000, + 0x00000000, + + /* 11 Chip Erase */ + + FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xC7, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0), + + }, + }, + + .page_size = 256u, + .sector_size = 4u * 1024u, + .blocksize = 32u * 1024u, + .is_uniform_blocksize = false, +}; +#else +# error Boot Flash type not chosen! +#endif diff --git a/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.h b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.h new file mode 100644 index 00000000000..b99c135ea37 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/imxrt_flexspi_nor_flash.h @@ -0,0 +1,349 @@ +/**************************************************************************** + * config/imxrt1060-evk/src/imxrt_flexspi_nor_flash.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Ivan Ucherdzhiev + * 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 __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H +#define __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* FLEXSPI memory config block related defintions */ + +#define FLEXSPI_CFG_BLK_TAG (0x42464346ul) +#define FLEXSPI_CFG_BLK_VERSION (0x56010400ul) +#define FLEXSPI_CFG_BLK_SIZE (512) + +/* FLEXSPI Feature related definitions */ + +#define FLEXSPI_FEATURE_HAS_PARALLEL_MODE 1 + +/* Lookup table related defintions */ + +#define CMD_INDEX_READ 0 +#define CMD_INDEX_READSTATUS 1 +#define CMD_INDEX_WRITEENABLE 2 +#define CMD_INDEX_WRITE 4 + +#define CMD_LUT_SEQ_IDX_READ 0 +#define CMD_LUT_SEQ_IDX_READSTATUS 1 +#define CMD_LUT_SEQ_IDX_WRITEENABLE 3 +#define CMD_LUT_SEQ_IDX_WRITE 9 + +#define CMD_SDR 0x01 +#define CMD_DDR 0x21 +#define RADDR_SDR 0x02 +#define RADDR_DDR 0x22 +#define CADDR_SDR 0x03 +#define CADDR_DDR 0x23 +#define MODE1_SDR 0x04 +#define MODE1_DDR 0x24 +#define MODE2_SDR 0x05 +#define MODE2_DDR 0x25 +#define MODE4_SDR 0x06 +#define MODE4_DDR 0x26 +#define MODE8_SDR 0x07 +#define MODE8_DDR 0x27 +#define WRITE_SDR 0x08 +#define WRITE_DDR 0x28 +#define READ_SDR 0x09 +#define READ_DDR 0x29 +#define LEARN_SDR 0x0a +#define LEARN_DDR 0x2a +#define DATSZ_SDR 0x0b +#define DATSZ_DDR 0x2b +#define DUMMY_SDR 0x0c +#define DUMMY_DDR 0x2c +#define DUMMY_RWDS_SDR 0x0d +#define DUMMY_RWDS_DDR 0x2d +#define JMP_ON_CS 0x1f +#define STOP 0 + +#define FLEXSPI_1PAD 0 +#define FLEXSPI_2PAD 1 +#define FLEXSPI_4PAD 2 +#define FLEXSPI_8PAD 3 + +#define FLEXSPI_LUT_OPERAND0_MASK (0xffu) +#define FLEXSPI_LUT_OPERAND0_SHIFT (0U) +#define FLEXSPI_LUT_OPERAND0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND0_SHIFT)) & \ + FLEXSPI_LUT_OPERAND0_MASK) +#define FLEXSPI_LUT_NUM_PADS0_MASK (0x300u) +#define FLEXSPI_LUT_NUM_PADS0_SHIFT (8u) +#define FLEXSPI_LUT_NUM_PADS0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS0_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS0_MASK) +#define FLEXSPI_LUT_OPCODE0_MASK (0xfc00u) +#define FLEXSPI_LUT_OPCODE0_SHIFT (10u) +#define FLEXSPI_LUT_OPCODE0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPCODE0_SHIFT)) & \ + FLEXSPI_LUT_OPCODE0_MASK) +#define FLEXSPI_LUT_OPERAND1_MASK (0xff0000u) +#define FLEXSPI_LUT_OPERAND1_SHIFT (16U) +#define FLEXSPI_LUT_OPERAND1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND1_SHIFT)) & \ + FLEXSPI_LUT_OPERAND1_MASK) +#define FLEXSPI_LUT_NUM_PADS1_MASK (0x3000000u) +#define FLEXSPI_LUT_NUM_PADS1_SHIFT (24u) +#define FLEXSPI_LUT_NUM_PADS1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS1_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS1_MASK) +#define FLEXSPI_LUT_OPCODE1_MASK (0xfc000000u) +#define FLEXSPI_LUT_OPCODE1_SHIFT (26u) +#define FLEXSPI_LUT_OPCODE1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE1_SHIFT)) & \ + FLEXSPI_LUT_OPCODE1_MASK) + +#define FLEXSPI_LUT_SEQ(cmd0, pad0, op0, cmd1, pad1, op1) \ + (FLEXSPI_LUT_OPERAND0(op0) | FLEXSPI_LUT_NUM_PADS0(pad0) | \ + FLEXSPI_LUT_OPCODE0(cmd0) | FLEXSPI_LUT_OPERAND1(op1) | \ + FLEXSPI_LUT_NUM_PADS1(pad1) | FLEXSPI_LUT_OPCODE1(cmd1)) + +/* */ + +#define NOR_CMD_INDEX_READ CMD_INDEX_READ +#define NOR_CMD_INDEX_READSTATUS CMD_INDEX_READSTATUS +#define NOR_CMD_INDEX_WRITEENABLE CMD_INDEX_WRITEENABLE +#define NOR_CMD_INDEX_ERASESECTOR 3 +#define NOR_CMD_INDEX_PAGEPROGRAM CMD_INDEX_WRITE +#define NOR_CMD_INDEX_CHIPERASE 5 +#define NOR_CMD_INDEX_DUMMY 6 +#define NOR_CMD_INDEX_ERASEBLOCK 7 + +/* READ LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ CMD_LUT_SEQ_IDX_READ + +/* Read Status LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS CMD_LUT_SEQ_IDX_READSTATUS + +/* 2 Read status DPI/QPI/OPI sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS_XPI 2 + +/* 3 Write Enable sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE CMD_LUT_SEQ_IDX_WRITEENABLE + +/* 4 Write Enable DPI/QPI/OPI sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE_XPI 4 + +/* 5 Erase Sector sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASESECTOR 5 + +/* 8 Erase Block sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASEBLOCK 8 + +/* 9 Program sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_PAGEPROGRAM CMD_LUT_SEQ_IDX_WRITE + +/* 11 Chip Erase sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_CHIPERASE 11 + +/* 13 Read SFDP sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ_SFDP 13 + +/* 14 Restore 0-4-4/0-8-8 mode sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_RESTORE_NOCMD 14 + +/* 15 Exit 0-4-4/0-8-8 mode sequence id in lookupTable stored in config blobk */ + +#define NOR_CMD_LUT_SEQ_IDX_EXIT_NOCMD 15 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Definitions for FlexSPI Serial Clock Frequency */ + +enum flexspi_serial_clkfreq_e { + FLEXSPI_SERIAL_CLKFREQ_30MHz = 1, + FLEXSPI_SERIAL_CLKFREQ_50MHz = 2, + FLEXSPI_SERIAL_CLKFREQ_60MHz = 3, + FLEXSPI_SERIAL_CLKFREQ_75MHz = 4, + FLEXSPI_SERIAL_CLKFREQ_80MHz = 5, + FLEXSPI_SERIAL_CLKFREQ_100MHz = 6, + FLEXSPI_SERIAL_CLKFREQ_133MHz = 7, + FLEXSPI_SERIAL_CLKFREQ_166MHz = 8, + FLEXSPI_SERIAL_CLKFREQ_200MHz = 9, +}; + +/* FlexSPI clock configuration type*/ + +enum flexspi_serial_clockmode_e { + FLEXSPI_CLKMODE_SDR, + FLEXSPI_CLKMODE_DDR, +}; + +/* FlexSPI Read Sample Clock Source definition */ + +enum flash_read_sample_clk_e { + FLASH_READ_SAMPLE_CLK_LOOPBACK_INTERNELLY = 0, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_DQSPAD = 1, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD = 2, + FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD = 3, +}; + +/* Misc feature bit definitions */ + +enum flash_misc_feature_e { + FLEXSPIMISC_OFFSET_DIFFCLKEN = 0, /* Bit for Differential clock enable */ + FLEXSPIMISC_OFFSET_CK2EN = 1, /* Bit for CK2 enable */ + FLEXSPIMISC_OFFSET_PARALLELEN = 2, /* Bit for Parallel mode enable */ + FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN = 3, /* Bit for Word Addressable enable */ + FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN = 4, /* Bit for Safe Configuration Frequency enable */ + FLEXSPIMISC_OFFSET_PAD_SETTING_OVERRIDE_EN = 5, /* Bit for Pad setting override enable */ + FLEXSPIMISC_OFFSET_DDR_MODE_EN = 6, /* Bit for DDR clock confiuration indication. */ +}; + +/* Flash Type Definition */ + +enum flash_flash_type_e { + FLEXSPI_DEVICE_TYPE_SERIAL_NOR = 1, /* Flash devices are Serial NOR */ + FLEXSPI_DEVICE_TYPE_SERIAL_NAND = 2, /* Flash devices are Serial NAND */ + FLEXSPI_DEVICE_TYPE_SERIAL_RAM = 3, /* Flash devices are Serial RAM/HyperFLASH */ + FLEXSPI_DEVICE_TYPE_MCP_NOR_NAND = 0x12, /* Flash device is MCP device, A1 is Serial NOR, A2 is Serial NAND */ + FLEXSPI_DEVICE_TYPE_MCP_NOR_RAM = 0x13, /* Flash deivce is MCP device, A1 is Serial NOR, A2 is Serial RAMs */ +}; + +/* Flash Pad Definitions */ + +enum flash_flash_pad_e { + SERIAL_FLASH_1PAD = 1, + SERIAL_FLASH_2PADS = 2, + SERIAL_FLASH_4PADS = 4, + SERIAL_FLASH_8PADS = 8, +}; + +/* Flash Configuration Command Type */ + +enum flash_config_cmd_e { + DEVICE_CONFIG_CMD_TYPE_GENERIC, /* Generic command, for example: configure dummy cycles, drive strength, etc */ + DEVICE_CONFIG_CMD_TYPE_QUADENABLE, /* Quad Enable command */ + DEVICE_CONFIG_CMD_TYPE_SPI2XPI, /* Switch from SPI to DPI/QPI/OPI mode */ + DEVICE_CONFIG_CMD_TYPE_XPI2SPI, /* Switch from DPI/QPI/OPI to SPI mode */ + DEVICE_CONFIG_CMD_TYPE_SPI2NO_CMD, /* Switch to 0-4-4/0-8-8 mode */ + DEVICE_CONFIG_CMD_TYPE_RESET, /* Reset device command */ +}; + +/* FlexSPI LUT Sequence structure */ + +struct flexspi_lut_seq_s { + uint8_t seq_num; /* Sequence Number, valid number: 1-16 */ + uint8_t seq_id; /* Sequence Index, valid number: 0-15 */ + uint16_t reserved; +}; + +/* FlexSPI Memory Configuration Block */ + +struct flexspi_mem_config_s { + uint32_t tag; + uint32_t version; + uint32_t reserved0; + uint8_t read_sample_clksrc; + uint8_t cs_hold_time; + uint8_t cs_setup_time; + uint8_t column_address_width; /* [0x00f-0x00f] Column Address with, for + * HyperBus protocol, it is fixed to 3, For + * Serial NAND, need to refer to datasheet */ + uint8_t device_mode_cfg_enable; + uint8_t device_mode_type; + uint16_t wait_time_cfg_commands; + struct flexspi_lut_seq_s device_mode_seq; + uint32_t device_mode_arg; + uint8_t config_cmd_enable; + uint8_t config_mode_type[3]; + struct flexspi_lut_seq_s config_cmd_seqs[3]; + uint32_t reserved1; + uint32_t config_cmd_args[3]; + uint32_t reserved2; + uint32_t controller_misc_option; + uint8_t device_type; + uint8_t sflash_pad_type; + uint8_t serial_clk_freq; + uint8_t lut_custom_seq_enable; + uint32_t reserved3[2]; + uint32_t sflash_a1size; + uint32_t sflash_a2size; + uint32_t sflash_b1size; + uint32_t sflash_b2size; + uint32_t cspad_setting_override; + uint32_t sclkpad_setting_override; + uint32_t datapad_setting_override; + uint32_t dqspad_setting_override; + uint32_t timeout_in_ms; + uint32_t command_interval; + uint16_t data_valid_time[2]; + uint16_t busy_offset; + uint16_t busybit_polarity; + uint32_t lookup_table[64]; + struct flexspi_lut_seq_s lut_customseq[12]; + uint32_t reserved4[4]; +}; + +/* Serial NOR configuration block */ + +struct flexspi_nor_config_s { + struct flexspi_mem_config_s mem_config; /* Common memory configuration info via FlexSPI */ + uint32_t page_size; /* Page size of Serial NOR */ + uint32_t sector_size; /* Sector size of Serial NOR */ + uint8_t ipcmd_serial_clkfreq; /* Clock frequency for IP command */ + uint8_t is_uniform_blocksize; /* Sector/Block size is the same */ + uint8_t reserved0[2]; /* Reserved for future use */ + uint8_t serial_nor_type; /* Serial NOR Flash type: 0/1/2/3 */ + uint8_t need_exit_nocmdmode; /* Need to exit NoCmd mode before other IP command */ + uint8_t halfclk_for_nonreadcmd; /* Half the Serial Clock for non-read command: true/false */ + uint8_t need_restore_nocmdmode; /* Need to Restore NoCmd mode after IP commmand execution */ + uint32_t blocksize; /* Block size */ + uint32_t reserve2[11]; /* Reserved for future use */ +}; + +#endif /* __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */ diff --git a/boards/nxp/fmurt1062-v1/src/init.c b/boards/nxp/fmurt1062-v1/src/init.c new file mode 100644 index 00000000000..4db3dfaf634 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/init.c @@ -0,0 +1,300 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019 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 init.c + * + * NXP imxrt1062-v1 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 "board_config.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "up_arch.h" +#include +#include "board_config.h" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * 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 + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + VDD_5V_PERIPH_EN(false); + VDD_5V_HIPOWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_5V_HIPOWER_EN(true); + VDD_5V_PERIPH_EN(true); + +} +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ + +__EXPORT void board_on_reset(int status) +{ + /* configure the GPIO pins to outputs and keep them low */ + + const uint32_t gpio[] = PX4_GPIO_PWM_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + if (status >= 0) { + up_mdelay(6); + } +} + +/**************************************************************************** + * Name: imxrt_boardinitialize + * + * Description: + * All i.MX RT architectures must provide the following entry point. This + * entry point is called early in the initialization -- after clocking and + * memory have been configured but before caches have been enabled and + * before any devices have been initialized. + * + ****************************************************************************/ + +__EXPORT void imxrt_boardinitialize(void) +{ + + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + + imxrt_spidev_initialize(); + + imxrt_usb_initialize(); + + fmurt1062_timer_initialize(); +} + +/**************************************************************************** + * 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) +{ + + /* Power on Interfaces */ + + + VDD_3V3_SD_CARD_EN(true); + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + VDD_3V3_SENSORS_EN(true); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + board_spi_reset(10); + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + + px4_platform_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* set up the serial DMA polling */ +#ifdef SERIAL_HAVE_DMA + 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)imxrt_serial_dma_poll, + NULL); +#endif + + /* initial LED state */ + drv_led_start(); + + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + +#if defined(CONFIG_IMXRT_USDHC) + int ret = fmurt1062_usdhc_initialize(); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + +#endif + + /* Configure SPI-based devices */ + + ret = imxrt1062_spi_bus_initialize(); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + + + return OK; +} + +// USB Stubs +#include +void up_usbinitialize(void) +{ +} + +int usbdev_register(struct usbdevclass_driver_s *driver) +{ + return -EINVAL; +} +int usbdev_unregister(struct usbdevclass_driver_s *driver) +{ + return -EINVAL; +} diff --git a/boards/nxp/fmurt1062-v1/src/led.c b/boards/nxp/fmurt1062-v1/src/led.c new file mode 100644 index 00000000000..9e1e0dbe37b --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/led.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2016, 2018 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 led.c + * + * NXP fmurt1062-v1 LED backend. + */ + +#include + +#include + +#include "chip.h" +#include +#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_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_nLED_GREEN, // 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++) { + if (g_ledmap[l] != 0) { + imxrt_config_gpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + imxrt_gpio_write(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + + if (g_ledmap[led] != 0) { + return imxrt_gpio_read(!g_ledmap[led]); + } + + return false; +} + +__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/boards/nxp/fmurt1062-v1/src/manifest.c b/boards/nxp/fmurt1062-v1/src/manifest.c new file mode 100644 index 00000000000..89c9327e21c --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/manifest.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "systemlib/px4_macros.h" +#include "px4_log.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_v0500[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0540[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { + {0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, + {0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + PX4_ERR("Board %4x is not supported!", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/nxp/fmurt1062-v1/src/sdhc.c b/boards/nxp/fmurt1062-v1/src/sdhc.c new file mode 100644 index 00000000000..df2f92cc1c3 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/sdhc.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 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. + * + ****************************************************************************/ +/* A micro Secure Digital (SD) card slot is available on the board connected to + * the SD Host Controller (USDHC1) signals of the MCU. This slot will accept + * micro format SD memory cards. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal IMXRT Pin + * ------------ ------------- -------- + * DAT0 USDHC1_DATA0 GPIO_SD_B0_02 + * DAT1 USDHC1_DATA1 GPIO_SD_B0_03 + * DAT2 USDHC1_DATA2 GPIO_SD_B0_04 + * CD/DAT3 USDHC1_DATA3 GPIO_SD_B0_05 + * CMD USDHC1_CMD GPIO_SD_B0_00 + * CLK USDHC1_CLK GPIO_SD_B0_01 + * CD USDHC1_CD GPIO_B1_12 + * ------------ ------------- -------- + * + * There are no Write Protect available to the IMXRT. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_usdhc.h" + +#include "board_config.h" + +#ifdef CONFIG_IMXRT_USDHC +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ +/**************************************************************************** + * Private Data + ****************************************************************************/ +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void) +{ + int ret; + + /* Mount the SDHC-based MMC/SD block driver */ + /* First, get an instance of the SDHC interface */ + + struct sdio_dev_s *sdhc = imxrt_usdhc_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdhc) { + PX4_ERR("ERROR: Failed to initialize SDHC slot %d\n", CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDHC interface to the MMC/SD driver */ + + ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdhc); + + if (ret != OK) { + PX4_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"); + + return OK; +} +#endif /* CONFIG_IMXRT_USDHC */ diff --git a/boards/nxp/fmurt1062-v1/src/spi.cpp b/boards/nxp/fmurt1062-v1/src/spi.cpp new file mode 100644 index 00000000000..bc3744c41f4 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/spi.cpp @@ -0,0 +1,423 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018 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 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include "imxrt_lpspi.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#include + +#if defined(CONFIG_IMXRT_LPSPI1) || defined(CONFIG_IMXRT_LPSPI2) || \ + defined(CONFIG_IMXRT_LPSPI3) || defined(CONFIG_IMXRT_LPSPI4) + +/* Define CS GPIO array */ + +#if defined(CONFIG_IMXRT_LPSPI1) +static const uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; +#endif +#if defined(CONFIG_IMXRT_LPSPI2) +static const uint32_t spi2selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; +#endif +#if defined(CONFIG_IMXRT_LPSPI3) +static const uint32_t spi3selects_gpio[] = PX4_BARO_BUS_CS_GPIO; +#endif +#if defined(CONFIG_IMXRT_LPSPI4) +static const uint32_t spi4selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +/************************************************************************************ + * Name: fmurt1062_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ + +void imxrt_spidev_initialize(void) +{ +#if defined(CONFIG_IMXRT_LPSPI1) + px4_gpio_init(spi1selects_gpio, arraySize(spi1selects_gpio)); +#endif +#if defined(CONFIG_IMXRT_LPSPI2) + px4_gpio_init(spi2selects_gpio, arraySize(spi2selects_gpio)); +#endif +#if defined(CONFIG_IMXRT_LPSPI3) + px4_gpio_init(spi3selects_gpio, arraySize(spi3selects_gpio)); +#endif +#if defined(CONFIG_IMXRT_LPSPI4) + px4_gpio_init(spi4selects_gpio, arraySize(spi4selects_gpio)); +#endif +} + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ +static struct spi_dev_s *spi_sensors; +static struct spi_dev_s *spi_memory; +static struct spi_dev_s *spi_baro; +static struct spi_dev_s *spi_ext; + +__EXPORT int imxrt1062_spi_bus_initialize(void) +{ + /* Configure SPI-based devices */ + + spi_sensors = px4_spibus_initialize(PX4_SPI_BUS_SENSORS); + + if (!spi_sensors) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + return -ENODEV; + } + + /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_sensors, 1 * 1000 * 1000); + SPI_SETBITS(spi_sensors, 8); + SPI_SETMODE(spi_sensors, SPIDEV_MODE3); + + for (int cs = PX4_SENSORS_BUS_FIRST_CS; cs <= PX4_SENSORS_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_sensors, cs, false); + } + + /* Get the SPI port for the Memory */ + + spi_memory = px4_spibus_initialize(PX4_SPI_BUS_MEMORY); + + if (!spi_memory) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_MEMORY); + return -ENODEV; + } + + /* Default PX4_SPI_BUS_MEMORY to 12MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_memory, 12 * 1000 * 1000); + SPI_SETBITS(spi_memory, 8); + SPI_SETMODE(spi_memory, SPIDEV_MODE3); + + for (int cs = PX4_MEMORY_BUS_FIRST_CS; cs <= PX4_MEMORY_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_memory, cs, false); + } + + /* Get the SPI port for the BARO */ + + spi_baro = px4_spibus_initialize(PX4_SPI_BUS_BARO); + + if (!spi_baro) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_BARO); + return -ENODEV; + } + + /* MS5611 has max SPI clock speed of 20MHz + */ + + SPI_SETFREQUENCY(spi_baro, 20 * 1000 * 1000); + SPI_SETBITS(spi_baro, 8); + SPI_SETMODE(spi_baro, SPIDEV_MODE3); + + for (int cs = PX4_BARO_BUS_FIRST_CS; cs <= PX4_BARO_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_baro, cs, false); + } + + /* Get the SPI port for the PX4_SPI_EXTERNAL1 */ + + spi_ext = px4_spibus_initialize(PX4_SPI_BUS_EXTERNAL1); + + if (!spi_ext) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL1); + return -ENODEV; + } + + /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000); + SPI_SETBITS(spi_ext, 8); + SPI_SETMODE(spi_ext, SPIDEV_MODE3); + + for (int cs = PX4_EXTERNAL1_BUS_FIRST_CS; cs <= PX4_EXTERNAL1_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_ext, cs, false); + } + + return OK; + +} + +/**************************************************************************** + * Name: imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status + * + * Description: + * The external functions, imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status must be + * provided by board-specific logic. They are implementations of the select + * and status methods of the SPI interface defined by struct spi_ops_s (see + * include/nuttx/spi/spi.h). All other methods (including imxrt_lpspibus_initialize()) + * are provided by common STM32 logic. To use this common SPI logic on your + * board: + * + * 1. Provide logic in imxrt_boardinitialize() to configure SPI chip select + * pins. + * 2. Provide imxrt_lpspi1/2/3select() and imxrt_lpspi1/2/3status() functions in your + * board-specific logic. These functions will perform chip selection and + * status operations using GPIOs in the way your board is configured. + * 3. Add a calls to imxrt_lpspibus_initialize() in your low level application + * initialization logic + * 4. The handle returned by imxrt_lpspibus_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). + * + ****************************************************************************/ + +#if defined(CONFIG_IMXRT_LPSPI1) +__EXPORT void imxrt_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* 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_SENSORS); + + /* Making sure the other peripherals are not selected */ + + for (auto cs : spi1selects_gpio) { + imxrt_gpio_write(cs, 1); + } + + uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + imxrt_gpio_write(gpio, !selected); + } +} + +__EXPORT uint8_t imxrt_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#if defined(CONFIG_IMXRT_LPSPI2) +__EXPORT void imxrt_lpspi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + int sel = (int) devid; + + if (devid == SPIDEV_FLASH(0)) { + sel = PX4_SPIDEV_MEMORY; + } + + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_MEMORY); + + /* Making sure the other peripherals are not selected */ + + for (auto cs : spi2selects_gpio) { + imxrt_gpio_write(cs, 1); + } + + uint32_t gpio = spi2selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + imxrt_gpio_write(gpio, !selected); + } +} + +__EXPORT uint8_t imxrt_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#if defined(CONFIG_IMXRT_LPSPI3) +__EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* 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_BARO); + + /* Making sure the other peripherals are not selected */ + + for (auto cs : spi3selects_gpio) { + imxrt_gpio_write(cs, 1); + } + + uint32_t gpio = spi3selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + imxrt_gpio_write(gpio, !selected); + } +} + +__EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#if defined(CONFIG_IMXRT_LPSPI4) +__EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + int sel = (int) devid; + + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXTERNAL1); + + /* Making sure the other peripherals are not selected */ + for (auto cs : spi4selects_gpio) { + imxrt_gpio_write(cs, 1); + } + + uint32_t gpio = spi4selects_gpio[PX4_SPI_DEV_ID(sel)]; + + if (gpio) { + imxrt_gpio_write(gpio, !selected); + } +} + +__EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +/************************************************************************************ + * Name: board_spi_reset + * + * Description: + * + * + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms) +{ +#ifdef CONFIG_IMXRT_LPSPI1 + + /* Goal not to back feed the chips on the bus via IO lines */ + for (auto cs : spi1selects_gpio) { + imxrt_config_gpio(_PIN_OFF(cs)); + } + + imxrt_config_gpio(GPIO_SPI1_SCK_OFF); + imxrt_config_gpio(GPIO_SPI1_MISO_OFF); + imxrt_config_gpio(GPIO_SPI1_MOSI_OFF); + + for (auto cs : spi3selects_gpio) { + imxrt_config_gpio(_PIN_OFF(cs)); + } + + imxrt_config_gpio(GPIO_SPI3_SCK_OFF); + imxrt_config_gpio(GPIO_SPI3_MISO_OFF); + imxrt_config_gpio(GPIO_SPI3_MOSI_OFF); + + + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SDA_RESET)); + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SCL_RESET)); + +# if BOARD_USE_DRDY + imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20689); + imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY2_BMI055_GYRO); + imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY3_BMI055_ACC); + imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY4_ICM20602); +# endif + /* set the sensor rail off */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ + for (auto cs : spi1selects_gpio) { + imxrt_config_gpio(cs); + } + + imxrt_config_gpio(GPIO_LPSPI1_SCK); + imxrt_config_gpio(GPIO_LPSPI1_MISO); + imxrt_config_gpio(GPIO_LPSPI1_MOSI); + + /* reconfigure the SPI pins */ + for (auto cs : spi3selects_gpio) { + imxrt_config_gpio(cs); + } + + imxrt_config_gpio(GPIO_LPSPI3_SCK); + imxrt_config_gpio(GPIO_LPSPI3_MISO); + imxrt_config_gpio(GPIO_LPSPI3_MOSI); + + imxrt_config_gpio(GPIO_LPI2C3_SDA); + imxrt_config_gpio(GPIO_LPI2C3_SCL); + +# if BOARD_USE_DRDY + imxrt_config_gpio(GPIO_SPI1_DRDY1_ICM20689); + imxrt_config_gpio(GPIO_SPI1_DRDY2_BMI055_GYRO); + imxrt_config_gpio(GPIO_SPI1_DRDY3_BMI055_ACC); + imxrt_config_gpio(GPIO_SPI1_DRDY4_ICM20602); +# endif +#endif /* CONFIG_IMXRT_LPSPI1 */ + +} + +#endif /* CONFIG_IMXRT_LPSPI1 || CONFIG_IMXRT_LPSPI2 || CONFIG_IMXRT_LPSPI3 || CONFIG_IMXRT_LPSPI4 */ diff --git a/boards/nxp/fmurt1062-v1/src/timer_config.c b/boards/nxp/fmurt1062-v1/src/timer_config.c new file mode 100644 index 00000000000..e9d0c5e3893 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/timer_config.c @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018-2019 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 timer_config.c + * + * Configuration data for the imxrt 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 "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" + +#include +#include + +#include "board_config.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) + +/* QTimer3 register accessors */ + +#define REG(_reg) _REG(IMXRT_QTIMER3_BASE + IMXRT_TMR_OFFSET(IMXRT_TMR_CH0,(_reg))) + +#define rCOMP1 REG(IMXRT_TMR_COMP1_OFFSET) +#define rCOMP2 REG(IMXRT_TMR_COMP2_OFFSET) +#define rCAPT REG(IMXRT_TMR_CAPT_OFFSET) +#define rLOAD REG(IMXRT_TMR_LOAD_OFFSET) +#define rHOLD REG(IMXRT_TMR_HOLD_OFFSET) +#define rCNTR REG(IMXRT_TMR_CNTR_OFFSET) +#define rCTRL REG(IMXRT_TMR_CTRL_OFFSET) +#define rSCTRL REG(IMXRT_TMR_SCTRL_OFFSET) +#define rCMPLD1 REG(IMXRT_TMR_CMPLD1_OFFSET) +#define rCMPLD2 REG(IMXRT_TMR_CMPLD2_OFFSET) +#define rCSCTRL REG(IMXRT_TMR_CSCTRL_OFFSET) +#define rFILT REG(IMXRT_TMR_FILT_OFFSET) +#define rDMA REG(IMXRT_TMR_DMA_OFFSET) +#define rENBL REG(IMXRT_TMR_ENBL_OFFSET) + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = IMXRT_FLEXPWM2_BASE, + .first_channel_index = 0, + .last_channel_index = 3, + }, + + { + .base = IMXRT_FLEXPWM3_BASE, + .first_channel_index = 4, + .last_channel_index = 5, + }, + { + .base = IMXRT_FLEXPWM4_BASE, + .first_channel_index = 6, + .last_channel_index = 7, + }, +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + /* FMU_CH1 : GPIO_B0_06 GPIO2 Pin 6 FLEXPWM2_PWMA0 */ + .gpio_out = PIN_FLEXPWM2_PWMA00, + .timer_index = 0, + .val_offset = PWMA_VAL, + .sub_module = SM0, + .sub_module_bits = MCTRL_LDOK(1 << SM0), + + }, + { + /* FMU_CH2 : GPIO_EMC_08 GPIO4 Pin 8 FLEXPWM2_PWMA1 */ + .gpio_out = PIN_FLEXPWM2_PWMA01, + .timer_index = 0, + .val_offset = PWMA_VAL, + .sub_module = SM1, + .sub_module_bits = MCTRL_LDOK(1 << SM1), + + }, + { + /* FMU_CH3 : GPIO_EMC_10 GPIO4 Pin 10 FLEXPWM2_PWMA2 */ + .gpio_out = PIN_FLEXPWM2_PWMA02, + .timer_index = 0, + .val_offset = PWMA_VAL, + .sub_module = SM2, + .sub_module_bits = MCTRL_LDOK(1 << SM2), + + }, + { + /* FMU_CH4 : GPIO_AD_B0_09 GPIO1 Pin 9 FLEXPWM2_PWMA3 */ + .gpio_out = PIN_FLEXPWM2_PWMA03, + .timer_index = 0, + .val_offset = PWMA_VAL, + .sub_module = SM3, + .sub_module_bits = MCTRL_LDOK(1 << SM3), + }, + + { + /* FMU_CH5 : GPIO_EMC_33 GPIO3 Pin 19 FLEXPWM3_PWMA2 */ + .gpio_out = PIN_FLEXPWM3_PWMA02, + .timer_index = 1, + .val_offset = PWMA_VAL, + .sub_module = SM2, + .sub_module_bits = MCTRL_LDOK(1 << SM2), + }, + { + /* FMU_CH6 : GPIO_EMC_30 GPIO4 Pin 30 FLEXPWM3_PWMB0 */ + .gpio_out = PIN_FLEXPWM3_PWMB00, + .timer_index = 1, + .val_offset = PWMB_VAL, + .sub_module = SM0, + .sub_module_bits = MCTRL_LDOK(1 << SM0), + + }, + + { + /* FMU_CH7 : GPIO_EMC_04 GPIO4 Pin 4 FLEXPWM4_PWMA2 */ + .gpio_out = PIN_FLEXPWM4_PWMA02, + .timer_index = 2, + .val_offset = PWMA_VAL, + .sub_module = SM2, + .sub_module_bits = MCTRL_LDOK(1 << SM2), + + }, + { + /* FMU_CH8 : GPIO_EMC_01 GPIO4 Pin 1 FLEXPWM4_PWMB0 */ + .gpio_out = PIN_FLEXPWM4_PWMB00, + .timer_index = 2, + .val_offset = PWMB_VAL, + .sub_module = SM0, + .sub_module_bits = MCTRL_LDOK(1 << SM0), + + }, + +}; + +__EXPORT const struct io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { +}; + +__EXPORT const struct timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +}; + + +__EXPORT void fmurt1062_timer_initialize(void) +{ + /* We must configure Qtimer 3 as the IPG divide by to yield 16 Mhz + * and deliver that clock to the eFlexPWM234 via XBAR + * + * IPG = 144 Mhz + * 16Mhz = 144 / 9 + * COMP 1 = 5, COMP2 = 4 + * + * */ + + /* Enable Block Clocks for Qtimer and XBAR1 */ + + imxrt_clockall_timer3(); + imxrt_clockall_xbar1(); + + /* Disable Timer */ + + rCTRL = 0; + rCOMP1 = 5 - 1; // N - 1 + rCOMP2 = 4 - 1; + + rCAPT = 0; + rLOAD = 0; + rCNTR = 0; + + rSCTRL = TMR_SCTRL_OEN; + + rCMPLD1 = 0; + rCMPLD2 = 0; + rCSCTRL = 0; + rFILT = 0; + rDMA = 0; + + /* Count rising edges of primary source, + * Prescaler is /1 + * Count UP until compare, then re-initialize. a successful compare occurs when the counter reaches a COMP1 value. + * Toggle OFLAG output using alternating compare registers + */ + rCTRL = (TMR_CTRL_CM_MODE1 | TMR_CTRL_PCS_DIV1 | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE_TOG_ALT); + + /* QTIMER3_TIMER0 -> Flexpwm234ExtClk */ + + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM234_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); +} diff --git a/boards/nxp/fmurt1062-v1/src/usb.c b/boards/nxp/fmurt1062-v1/src/usb.c new file mode 100644 index 00000000000..5ef6169180c --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/usb.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018 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 usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" +#include "imxrt_periphclks.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int imxrt_usb_initialize(void) +{ + imxrt_clockall_usboh3(); + return 0; +} +/************************************************************************************ + * Name: imxrt_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 imxrt_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 imxrt_usbpullup(FAR struct usbdev_s *dev, bool enable) +{ + usbtrace(TRACE_DEVPULLUP, (uint16_t)enable); + + return OK; +} + +/************************************************************************************ + * Name: imxrt_usbsuspend + * + * Description: + * Board logic must provide the imxrt_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 imxrt_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + +/************************************************************************************ + * Name: board_read_VBUS_state + * + * Description: + * All boards must provide a way to read the state of VBUS, this my be simple + * digital input on a GPIO. Or something more complicated like a Analong input + * or reading a bit from a USB controller register. + * + * Returns - 0 if connected. + * + ************************************************************************************/ + +int board_read_VBUS_state(void) +{ + + return (getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1; +} diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 9d87fcf9bdd..3050deecd4e 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -380,6 +380,7 @@ typedef enum PX4_SOC_ARCH_ID_t { PX4_SOC_ARCH_ID_STM32F7 = 0x0002, PX4_SOC_ARCH_ID_KINETISK66 = 0x0003, PX4_SOC_ARCH_ID_SAMV7 = 0x0004, + PX4_SOC_ARCH_ID_NXPIMXRT1062 = 0x0005, PX4_SOC_ARCH_ID_STM32H7 = 0x0006, diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 720b589d5c8..acb9fa0b7fa 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -178,7 +178,10 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.g # vscode launch.json # FIXME: hack to skip if px4_io-v2 because it's a built within another build (eg px4_fmu-v5) if(NOT PX4_BOARD MATCHES "px4_io-v2") - if(CONFIG_ARCH_CHIP_MK66FN2M0VMD18) + if(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) + set(DEBUG_DEVICE "MIMXRT1062XXX6A") + set(DEBUG_SVD_FILE "MIMXRT1052.svd") + elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18) set(DEBUG_DEVICE "MK66FN2M0xxx18") set(DEBUG_SVD_FILE "MK66F18.svd") elseif(CONFIG_ARCH_CHIP_STM32F100C8) diff --git a/platforms/nuttx/NuttX/Make.defs.in b/platforms/nuttx/NuttX/Make.defs.in index 3a7165f7452..09d3e01ec6a 100644 --- a/platforms/nuttx/NuttX/Make.defs.in +++ b/platforms/nuttx/NuttX/Make.defs.in @@ -64,6 +64,11 @@ ifeq ($(WINTOOL),y) MKDEP = $(TOPDIR)/tools/mknulldeps.sh endif +ifeq ($(CONFIG_BOARD_USE_PROBES),y) + ARCHINCLUDES += -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) + ARCHXXINCLUDES += -I$(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) +endif + FLAGS = -Os -g2 \ -fdata-sections \ -ffunction-sections \ diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index c6ba2c13c79..7b36108b548 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit c6ba2c13c799dcb18b53aaf63978925473cab7ce +Subproject commit 7b36108b548642f1a8513dcf00eb4913f8558ccf diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 8a601eb6f0e..31c45cd4e49 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -118,6 +118,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18) set(CHIP_MANUFACTURER "nxp") set(CHIP "k66") + elseif(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) + set(CHIP_MANUFACTURER "nxp") + set(CHIP "rt106x") else() message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.") endif() diff --git a/platforms/nuttx/src/px4/nxp/imxrt/adc/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/adc/CMakeLists.txt new file mode 100644 index 00000000000..d2487d05bf2 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/adc/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp new file mode 100644 index 00000000000..ec601d8882c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * Copyright (C) 2018-2019 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 + * + * Driver for the imxrt 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 + +typedef uint32_t adc_chan_t; +#define ADC_TOTAL_CHANNELS 16 + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* ADC register accessors */ + +#define REG(base_address, _reg) _REG((base_address) + (_reg)) + +#define rHC0(base_address) REG(base_address, IMXRT_ADC_HC0_OFFSET) /* Control register for hardware triggers */ +#define rHC1(base_address) REG(base_address, IMXRT_ADC_HC1_OFFSET) /* Control register for hardware triggers */ +#define rHC2(base_address) REG(base_address, IMXRT_ADC_HC2_OFFSET) /* Control register for hardware triggers */ +#define rHC3(base_address) REG(base_address, IMXRT_ADC_HC3_OFFSET) /* Control register for hardware triggers */ +#define rHC4(base_address) REG(base_address, IMXRT_ADC_HC4_OFFSET) /* Control register for hardware triggers */ +#define rHC5(base_address) REG(base_address, IMXRT_ADC_HC5_OFFSET) /* Control register for hardware triggers */ +#define rHC6(base_address) REG(base_address, IMXRT_ADC_HC6_OFFSET) /* Control register for hardware triggers */ +#define rHC7(base_address) REG(base_address, IMXRT_ADC_HC7_OFFSET) /* Control register for hardware triggers */ +#define rHS(base_address) REG(base_address, IMXRT_ADC_HS_OFFSET) /* Status register for HW triggers */ +#define rR0(base_address) REG(base_address, IMXRT_ADC_R0_OFFSET) /* Data result register for HW triggers */ +#define rR1(base_address) REG(base_address, IMXRT_ADC_R1_OFFSET) /* Data result register for HW triggers */ +#define rR2(base_address) REG(base_address, IMXRT_ADC_R2_OFFSET) /* Data result register for HW triggers */ +#define rR3(base_address) REG(base_address, IMXRT_ADC_R3_OFFSET) /* Data result register for HW triggers */ +#define rR4(base_address) REG(base_address, IMXRT_ADC_R4_OFFSET) /* Data result register for HW triggers */ +#define rR5(base_address) REG(base_address, IMXRT_ADC_R5_OFFSET) /* Data result register for HW triggers */ +#define rR6(base_address) REG(base_address, IMXRT_ADC_R6_OFFSET) /* Data result register for HW triggers */ +#define rR7(base_address) REG(base_address, IMXRT_ADC_R7_OFFSET) /* Data result register for HW triggers */ +#define rCFG(base_address) REG(base_address, IMXRT_ADC_CFG_OFFSET) /* Configuration register */ +#define rGC(base_address) REG(base_address, IMXRT_ADC_GC_OFFSET) /* General control register */ +#define rGS(base_address) REG(base_address, IMXRT_ADC_GS_OFFSET) /* General status register */ +#define rCV(base_address) REG(base_address, IMXRT_ADC_CV_OFFSET) /* Compare value register */ +#define rOFS(base_address) REG(base_address, IMXRT_ADC_OFS_OFFSET) /* Offset correction value register */ +#define rCAL(base_address) REG(base_address, IMXRT_ADC_CAL_OFFSET) /* Calibration value register */ + + +int px4_arch_adc_init(uint32_t base_address) +{ + static bool once = false; + + if (!once) { + + once = true; + + /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + + irqstate_t flags = px4_enter_critical_section(); + + imxrt_clockall_adc1(); + + rCFG(base_address) = ADC_CFG_ADICLK_IPGDIV2 | ADC_CFG_MODE_12BIT | \ + ADC_CFG_ADIV_DIV8 | ADC_CFG_ADLSMP | ADC_CFG_ADSTS_6_20 | \ + ADC_CFG_AVGS_4SMPL | ADC_CFG_OVWREN; + px4_leave_critical_section(flags); + + /* Clear the CALF and begin the calibration */ + + rGS(base_address) = ADC_GS_CALF; + rGC(base_address) = ADC_GC_CAL; + uint32_t guard = 100; + + while (guard != 0 && (rGS(base_address) & ADC_GC_CAL) == 0) { + guard--; + usleep(1); + } + + while ((rGS(base_address) & ADC_GC_CAL) == ADC_GC_CAL) { + + usleep(100); + + if (rGS(base_address) & ADC_GS_CALF) { + return -1; + } + } + + if ((rHS(base_address) & ADC_HS_COCO0) == 0) { + return -2; + } + + if (rGS(base_address) & ADC_GS_CALF) { + return -3; + } + + /* dummy read to clear COCO of calibration */ + + int32_t r = rR0(base_address); + UNUSED(r); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rGC(base_address) = ADC_GC_AVGE; + rHC0(base_address) = 0xd; // VREFSH = internal channel, for ADC self-test, hard connected to VRH internally + + while (!(rHS(base_address) & ADC_HS_COCO0)) { + + /* 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) { + return -4; + } + } + + r = rR0(base_address); + } // once + + return 0; +} +void px4_arch_adc_uninit(uint32_t base_address) +{ + imxrt_clockoff_adc1(); +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + + /* clear any previous COCO0 */ + + uint16_t result = rR0(base_address); + + rHC0(base_address) = channel; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + + while (!(rHS(base_address) & ADC_HS_COCO0)) { + /* 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) { + return 0xffff; + } + } + + /* read the result and clear COCO0 */ + result = rR0(base_address); + return result; +} +uint32_t px4_arch_adc_temp_sensor_mask() +{ + return 0; +} + +uint32_t px4_arch_adc_dn_fullcount(void) +{ + return 1 << 12; // 12 bit ADC +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/CMakeLists.txt new file mode 100644 index 00000000000..26cdb4bbc2c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2019 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_library(arch_board_critmon + board_critmon.c +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c new file mode 100644 index 00000000000..927adea9308 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c @@ -0,0 +1,79 @@ +/************************************************************************************ + * + * Copyright (C) 2018 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include "dwt.h" +#include "up_arch.h" + +#include + +#include + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +uint32_t up_critmon_gettime(void) +{ + return getreg32(DWT_CYCCNT); +} + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +{ + b32_t b32elapsed; + + b32elapsed = itob32(elapsed) / BOARD_CPU_FREQUENCY; + ts->tv_sec = b32toi(b32elapsed); + ts->tv_nsec = NSEC_PER_SEC * b32frac(b32elapsed) / b32ONE; +} + +#endif /* CONFIG_SCHED_CRITMONITOR */ diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt new file mode 100644 index 00000000000..3a466715b1f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2019 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_library(arch_board_hw_info + board_hw_rev_ver.c +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c new file mode 100644 index 00000000000..1ee7f75a44f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -0,0 +1,346 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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_hw_rev_ver.c + * Implementation of IMXRT based Board Hardware Revision and Version ID API + */ +#include +#include +#include +#include +#include +#include + +#include + +#if defined(BOARD_HAS_HW_VERSIONING) + +# if defined(GPIO_HW_VER_REV_DRIVE) +# define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE +# define GPIO_HW_VER_DRIVE GPIO_HW_VER_REV_DRIVE +# endif +/**************************************************************************** + * Private Data + ****************************************************************************/ +static int hw_version = 0; +static int hw_revision = 0; +static char hw_info[] = HW_INFO_INIT; + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ +/**************************************************************************** + * Name: determin_hw_version + * + * Description: + * + * This function fist determines if revision and version resistors are in place. + * if they it will read the ADC channels and decode the DN to ordinal numbers + * that will be returned by board_get_hw_version and board_get_hw_revision API + * + * This will return OK on success and -1 on not supported +* + * + ****************************************************************************/ + +static int dn_to_ordinal(uint16_t dn) +{ + + const struct { + uint16_t low; // High(n-1) + 1 + uint16_t high; // Average High(n)+Low(n+1) EX. 1356 = AVRG(1331,1382) + } dn2o[] = { + // R1(up) R2(down) V min V Max DN Min DN Max + {0, 0 }, // 0 No Resistors + {1, 579 }, // 1 24.9K 442K 0.166255191 0.44102252 204 553 + {580, 967 }, // 2 32.4K 174K 0.492349322 0.770203609 605 966 + {968, 1356}, // 3 38.3K 115K 0.787901749 1.061597759 968 1331 + {1357, 1756}, // 4 46.4K 84.5K 1.124833577 1.386007306 1382 1738 + {1757, 2137}, // 5 51.1K 61.9K 1.443393279 1.685367869 1774 2113 + {2138, 2519}, // 6 61.9K 51.1K 1.758510242 1.974702534 2161 2476 + {2520, 2919}, // 7 84.5K 46.4K 2.084546498 2.267198261 2562 2842 + {2920, 3308}, // 8 115K 38.3K 2.437863827 2.57656294 2996 3230 + {3309, 3699}, // 9 174K 32.4K 2.755223792 2.847933804 3386 3571 + {3700, 4095}, // 10 442K 24.9K 3.113737849 3.147347506 3827 3946 + }; + + for (unsigned int i = 0; i < arraySize(dn2o); i++) { + if (dn >= dn2o[i].low && dn <= dn2o[i].high) { + return i; + } + } + + return -1; +} + +/************************************************************************************ + * Name: read_id_dn + * + * Description: + * Read the HW sense set to get a DN of the value formed by + * 0 VDD + * | + * / + * \ R1 + * / + * | + * +--------------- GPIO_HW_xxx_SENCE | ADC channel N + * | + * / + * \ R2 + * / + * | + * | + * +--------------- GPIO_HW_xxx_DRIVE or GPIO_HW_VER_REV_DRIVE + * + * Input Parameters: + * id - pointer to receive the dn for the id set + * gpio_drive - gpio that is the drive + * gpio_sense - gpio that is the sence + * adc_channel - the Channel number associated with gpio_sense + * + * Returned Value: + * 0 - Success and id is set + * -EIO - FAiled to init or read the ADC + * + ************************************************************************************/ + +static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel) +{ + int rv = -EIO; + const unsigned int samples = 16; + /* + * Step one is there resistors? + * + * If we set the mid-point of the ladder which is the ADC input to an + * output, then whatever state is driven out should be seen by the GPIO + * that is on the bottom of the ladder that is switched to an input. + * The SENCE line is effectively an output with a high value pullup + * resistor on it driving an input through a series resistor with a pull up. + * If present the series resistor will form a low pass filter due to stray + * capacitance, but this is fine as long as we give it time to settle. + */ + + /* Turn the drive lines to digital inputs with No pull up */ + + imxrt_config_gpio(_MK_GPIO_INPUT(gpio_drive) & ~IOMUX_PULL_MASK); + + /* Turn the sense lines to digital outputs LOW */ + + imxrt_config_gpio(_MK_GPIO_OUTPUT(gpio_sense)); + + + up_udelay(100); /* About 10 TC assuming 485 K */ + + /* Read Drive lines while sense are driven low */ + + int low = imxrt_gpio_read(_MK_GPIO_INPUT(gpio_drive)); + + + /* Write the sense lines HIGH */ + + imxrt_gpio_write(_MK_GPIO_OUTPUT(gpio_sense), 1); + + up_udelay(100); /* About 10 TC assuming 485 K */ + + /* Read Drive lines while sense are driven high */ + + int high = imxrt_gpio_read(_MK_GPIO_INPUT(gpio_drive)); + + /* restore the pins to ANALOG */ + + imxrt_config_gpio(gpio_sense); + + /* Turn the drive lines to digital outputs LOW */ + + imxrt_config_gpio(gpio_drive ^ GPIO_OUTPUT_SET); + + up_udelay(100); /* About 10 TC assuming 485 K */ + + /* Are Resistors in place ?*/ + + uint32_t dn_sum = 0; + uint16_t dn = 0; + + if ((high ^ low) && low == 0) { + /* Yes - Fire up the ADC (it has once control) */ + + if (px4_arch_adc_init(HW_REV_VER_ADC_BASE) == OK) { + + /* Read the value */ + for (unsigned av = 0; av < samples; av++) { + dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel); + + if (dn == 0xffff) { + break; + } + + dn_sum += dn; + } + + if (dn != 0xffff) { + *id = dn_sum / samples; + rv = OK; + } + } + + } else { + /* No - No Resistors is ID 0 */ + *id = 0; + rv = OK; + } + + /* Turn the drive lines to digital outputs High */ + + imxrt_config_gpio(gpio_drive); + return rv; +} + + +static int determine_hw_info(int *revision, int *version) +{ + int dn; + int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL); + + if (rv == OK) { + *revision = dn_to_ordinal(dn); + rv = read_id_dn(&dn, GPIO_HW_VER_DRIVE, GPIO_HW_VER_SENSE, ADC_HW_VER_SENSE_CHANNEL); + + if (rv == OK) { + *version = dn_to_ordinal(dn); + } + } + + return rv; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * Name: board_get_hw_type + * + * Description: + * Optional returns a 0 terminated string defining the HW type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This my be a 0 length string "" + * + ************************************************************************************/ + +__EXPORT const char *board_get_hw_type_name() +{ + return (const char *) hw_info; +} + +/************************************************************************************ + * Name: board_get_hw_version + * + * Description: + * Optional returns a integer HW version + * + * Input Parameters: + * None + * + * Returned Value: + * An integer value of this boards hardware version. + * A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API. + * A value of 0 is the default for boards supporting the API but not having version. + * + ************************************************************************************/ + +__EXPORT int board_get_hw_version() +{ + return hw_version; +} + +/************************************************************************************ + * Name: board_get_hw_revision + * + * Description: + * Optional returns a integer HW revision + * + * Input Parameters: + * None + * + * Returned Value: + * An integer value of this boards hardware revision. + * A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API. + * A value of 0 is the default for boards supporting the API but not having revision. + * + ************************************************************************************/ + +__EXPORT int board_get_hw_revision() +{ + return hw_revision; +} + +/************************************************************************************ + * Name: board_determine_hw_info + * + * Description: + * Uses the HW revision and version detection added in FMUv5. + * See https://docs.google.com/spreadsheets/d/1-n0__BYDedQrc_2NHqBenG1DNepAgnHpSGglke-QQwY + * HW REV and VER ID tab. + * + * Input Parameters: + * None + * + * Returned Value: + * 0 - on success or negated errono + * 1) The values for integer value of this boards hardware revision is set + * 2) The integer value of this boards hardware version is set. + * 3) hw_info is populated + * + * A value of 0 is the default for boards supporting the BOARD_HAS_HW_VERSIONING API. + * but not having R1 and R2. + * + ************************************************************************************/ + +int board_determine_hw_info() +{ + int rv = determine_hw_info(&hw_revision, &hw_version); + + if (rv == OK) { + hw_info[HW_INFO_INIT_REV] = board_get_hw_revision() + '0'; + hw_info[HW_INFO_INIT_VER] = board_get_hw_version() + '0'; + } + + return rv; +} +#endif diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt new file mode 100644 index 00000000000..e818a473362 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2019 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_library(arch_board_reset + board_reset.c +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.c b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.c new file mode 100644 index 00000000000..1175c42df6f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 IMXRT based Board RESET API + */ + +#include +#include +#include +#include +#include + +#define PX4_IMXRT_RTC_REBOOT_REG 3 // Must be common with bootloader and: + +#if CONFIG_IMXRT_RTC_MAGIC_REG == PX4_IMXRT_RTC_REBOOT_REG +# error CONFIG_IMXRT_RTC_MAGIC_REG can nt have the save value as PX4_IMXRT_RTC_REBOOT_REG +#endif + +int board_reset(int status) +{ + up_systemreset(); + return 0; +} + +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; + } + + putreg32(regvalue, IMXRT_SNVS_LPGPR(PX4_IMXRT_RTC_REBOOT_REG)); + return OK; +} + + +void board_system_reset(int status) +{ +#if defined(BOARD_HAS_ON_RESET) + board_on_reset(status); +#endif + +#ifdef CONFIG_BOARDCTL_RESET + board_reset(status); +#endif + + while (1); +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/hrt/CMakeLists.txt new file mode 100644 index 00000000000..3edd775b6a6 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_hrt + hrt.c +) +target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c new file mode 100644 index 00000000000..5587b49e6c2 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c @@ -0,0 +1,875 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019 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 + * Author: David Sidrane + * + * High-resolution timer callouts and timekeeping. + * + * This can use any GPT 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 Kinetis 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 +#include + + +#include "chip.h" +#include "hardware/imxrt_gpt.h" +#include "imxrt_periphclks.h" + +#undef PPM_DEBUG + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +#define CAT3_(A, B, C) A##B##C +#define CAT3(A, B, C) CAT3_(A, B, C) + +#ifdef HRT_TIMER + +#define HRT_TIMER_FREQ 1000000 + +/* HRT configuration */ + +#define HRT_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */ +#define HRT_TIMER_BASE CAT3(IMXRT_GPT, HRT_TIMER,_BASE) /* The Base address of the GPT */ +#define HRT_TIMER_VECTOR CAT(IMXRT_IRQ_GPT, HRT_TIMER) /* The GPT Interrupt vector */ + +#if HRT_TIMER == 1 +# define HRT_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */ +#elif HRT_TIMER == 2 +# define HRT_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */ +#endif + +#if HRT_TIMER == 1 && defined(CONFIG_IMXRT_GPT1) +# error must not set CONFIG_IMXRT_GPT1=y and HRT_TIMER=1 +#elif HRT_TIMER == 2 && defined(CONFIG_IMXRT_GPT2) +# error must not set CONFIG_IMXRT_GPT2=y and HRT_TIMER=2 +#endif + +/* +* HRT clock must be a multiple of 1MHz greater than 1MHz +*/ +#if (HRT_TIMER_CLOCK % HRT_TIMER_FREQ) != 0 +# error HRT_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if HRT_TIMER_CLOCK <= HRT_TIMER_FREQ +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/** +* Minimum/maximum deadlines. +* +* These are suitable for use with a 32-bit timer/counter clocked +* at 1MHz. The high-resolution timer need only guarantee that it +* not wrap more than once in the 4294.967296s 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 4294951760LL + +/* +* Period of the free-running counter, in microseconds. +*/ +#define HRT_COUNTER_PERIOD 4294967296LL + +/* +* Scaling factor(s) for the free-running counter; convert an input +* in counts to a time in microseconds. +*/ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* Timer register accessors */ + +#define REG(_reg) _REG(HRT_TIMER_BASE + (_reg)) + +#define rCR REG(IMXRT_GPT_CR_OFFSET) +#define rPR REG(IMXRT_GPT_PR_OFFSET) +#define rSR REG(IMXRT_GPT_SR_OFFSET) +#define rIR REG(IMXRT_GPT_IR_OFFSET) +#define rOCR1 REG(IMXRT_GPT_OCR1_OFFSET) +#define rOCR2 REG(IMXRT_GPT_OCR2_OFFSET) +#define rOCR3 REG(IMXRT_GPT_OCR3_OFFSET) +#define rICR1 REG(IMXRT_GPT_ICR1_OFFSET) +#define rICR2 REG(IMXRT_GPT_ICR2_OFFSET) +#define rCNT REG(IMXRT_GPT_CNT_OFFSET) + +/* +* Specific registers and bits used by HRT sub-functions +*/ + +# define rOCR_HRT CAT(rOCR, HRT_TIMER_CHANNEL) /* GPT Output Compare Register used by HRT */ +# define STATUS_HRT CAT(GPT_SR_OF, HRT_TIMER_CHANNEL) /* OF Output Compare Flag */ +# define OFIE_HRT CAT3(GPT_IR_OF, HRT_TIMER_CHANNEL,IE) /* Output Compare Interrupt Enable */ + +#if (HRT_TIMER_CHANNEL > 1) || (HRT_TIMER_CHANNEL > 3) +# error HRT_TIMER_CHANNEL must be a value between 1 and 3 +#endif + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint32_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint32_t latency_actual; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context, void *args); +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); + +#if !defined(HRT_PPM_CHANNEL) + +/* When HRT_PPM_CHANNEL is not used provide null operations */ + +# define GPT_CR_IM_BOTH 0 +# define STATUS_PPM 0 +# define IFIE_PPM 0 + +#else + +/* Specific registers and bits used by PPM sub-functions */ + +# define rICR_PPM CAT(rICR, HRT_PPM_CHANNEL) /* GPT Input Capture Register used by PPL */ +# define GPT_CR_IM_BOTH CAT3(GPT_CR_IM, HRT_PPM_CHANNEL, _BOTH) /* GPT Capture mode both */ +# define STATUS_PPM CAT(GPT_SR_IF, HRT_PPM_CHANNEL) /* IF Input capture Flag */ +# define IFIE_PPM CAT3(GPT_IR_IF, HRT_PPM_CHANNEL,IE) /* Output Compare Interrupt Enable */ + +/* Sanity checking */ + +# if (HRT_PPM_CHANNEL != 1) && (HRT_PPM_CHANNEL != 2) +# error HRT_PPM_CHANNEL must be a value of 1 or 2 +# endif + +# if (HRT_PPM_CHANNEL == HRT_TIMER_CHANNEL) +# error HRT_PPM_CHANNEL must not be the same as HRT_TIMER_CHANNEL +# endif +/* + * 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; + + +# if defined(PPM_DEBUG) + +# define EDGE_BUFFER_COUNT 32 + +/* PPM edge history */ + +__EXPORT uint32_t ppm_edge_history[EDGE_BUFFER_COUNT]; +unsigned ppm_edge_next; + +/* PPM pulse history */ + +__EXPORT uint32_t ppm_pulse_history[EDGE_BUFFER_COUNT]; +unsigned ppm_pulse_next; +# endif + +static uint32_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/** PPM decoder state machine */ +struct { + uint32_t last_edge; /**< last capture time */ + uint32_t last_mark; /**< last significant edge */ + uint32_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); +#endif /* HRT_PPM_CHANNEL */ + +/** + * Initialize the timer we are going to use. + */ +static void hrt_tim_init(void) +{ + /* Enable the Module clock */ + + HRT_CLOCK_ALL(); + + + /* claim our interrupt vector */ + + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL); + + /* disable and configure the timer */ + + rCR = 0; + + rCR = GPT_CR_OM1_DIS | GPT_CR_OM2_DIS | GPT_CR_OM3_DIS | + GPT_CR_IM_BOTH | GPT_CR_FRR | GPT_CR_CLKSRC_IPG | GPT_CR_ENMOD; + + /* CLKSRC field is divided by [PRESCALER + 1] */ + + rPR = (HRT_TIMER_CLOCK / HRT_TIMER_FREQ) - 1; + + /* set an initial capture a little ways off */ + + rOCR_HRT = 1000; + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); + + rIR = IFIE_PPM | OFIE_HRT; + + /* enable the timer */ + + rCR |= GPT_CR_EN; + +} + +#ifdef HRT_PPM_CHANNEL +/** + * Handle the PPM decoder state machine. + */ +static void hrt_ppm_decode(uint32_t status) +{ + uint32_t count = rICR_PPM; + uint32_t width; + uint32_t interval; + unsigned i; + + /* how long since the last edge? - this handles counter wrapping implicitly. */ + width = count - ppm.last_edge; + +#if PPM_DEBUG + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= EDGE_BUFFER_COUNT) { + 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 >= EDGE_BUFFER_COUNT) { + 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, void *arg) +{ + /* grab the timer for latency tracking purposes */ + + latency_actual = rCNT; + + /* copy interrupt status */ + uint32_t status = rSR; + + /* ack the interrupts we just read */ + + rSR = status; + +#ifdef HRT_PPM_CHANNEL + + /* was this a PPM edge? */ + if (status & (STATUS_PPM)) { + hrt_ppm_decode(status); + } + +#endif + + /* was this a timer tick? */ + if (status & STATUS_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(const 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 as atomic operation. + */ +hrt_abstime +hrt_elapsed_time_atomic(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; +} + +/** + * Initialize 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 uninitialized + 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 uninitialized 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 *)(void *)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 *)(void *)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 *)(void *)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 *)(void *)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 32 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 %ul at %ul\n", (unsigned long)(deadline & 0xffffffff), (unsigned long)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + + rOCR_HRT = latency_baseline = deadline; + +} + +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/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/adc.h new file mode 100644 index 00000000000..b2b4c609675 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/adc.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ +#pragma once + +#include + +#if !defined(HW_REV_VER_ADC_BASE) +# define HW_REV_VER_ADC_BASE IMXRT_ADC1_BASE +#endif + +#if !defined(SYSTEM_ADC_BASE) +# define SYSTEM_ADC_BASE IMXRT_ADC1_BASE +#endif + +#include diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h new file mode 100644 index 00000000000..d8ebd7c9ef6 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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 + * + * imxrt-specific PWM output data. + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#define MAX_IO_TIMERS 4 +#define MAX_TIMER_IO_CHANNELS 16 + +#define MAX_LED_TIMERS 2 +#define MAX_TIMER_LED_CHANNELS 6 + +#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, + IOTimerChanMode_OneShot = 4, + IOTimerChanMode_Trigger = 5, + IOTimerChanModeSize +} io_timer_channel_mode_t; + +typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */ + +/* array of timers dedicated to PWM in and out and TBD capture use + *** Timers are driven from QTIMER3_OUT0 + *** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz + *** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz + *** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz + *** the resulting PSC will be one and the timer will count at it's clock frequency. + */ +typedef struct io_timers_t { + uint32_t base; /* Base address of the timer */ + uint32_t clock_register; /* SIM_SCGCn */ + uint32_t clock_bit; /* SIM_SCGCn bit pos */ + uint32_t vectorno; /* IRQ number */ + uint32_t first_channel_index; /* 0 based index in timer_io_channels */ + uint32_t last_channel_index; /* 0 based index in timer_io_channels */ + xcpt_t handler; +} io_timers_t; + +/* array of channels in logical order */ +typedef struct timer_io_channels_t { + uint32_t gpio_out; /* The timer valn_offset GPIO for PWM */ + uint32_t gpio_in; /* The timer valn_offset GPIO for Capture */ + uint8_t timer_index; /* 0 based index in the io_timers_t table */ + uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */ + uint8_t sub_module; /* 0 based sub module offset */ + uint8_t sub_module_bits; /* LDOK and CLDOK bits */ +} timer_io_channels_t; + +#define SM0 0 +#define SM1 1 +#define SM2 2 +#define SM3 3 + +#define PWMA_VAL IMXRT_FLEXPWM_SM0VAL3_OFFSET +#define PWMB_VAL IMXRT_FLEXPWM_SM0VAL5_OFFSET + + +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, + uint16_t capture); + + +/* 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, void *arg); +__EXPORT int io_timer_handler1(int irq, void *context, void *arg); +__EXPORT int io_timer_handler2(int irq, void *context, void *arg); +__EXPORT int io_timer_handler3(int irq, void *context, void *arg); + +__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_init_timer(unsigned timer); + +__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); +__EXPORT extern void io_timer_trigger(void); + +__END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/CMakeLists.txt new file mode 100644 index 00000000000..401bf06ef5d --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_io_pins + io_timer.c + pwm_servo.c + pwm_trigger.c + input_capture.c +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c new file mode 100644 index 00000000000..a5b0e3ce4cb --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c @@ -0,0 +1,330 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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 imxrt timer blocks. + * + * Works with any FLEXPWN that have input pins. + * + * Require an interrupt. + * + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include "hardware/imxrt_flexpwm.h" + +#define MAX_CHANNELS_PER_TIMER 2 + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define REG(_tmr, _sm, _reg) _REG16(io_timers[(_tmr)].base + ((_sm) * SM_SPACING), (_reg)) + +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) +{ + 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 = 0;//_REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF; + + 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 (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) +{ + return 0; +} +int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) +{ + return 0; +} + +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; + uint32_t offset = timer_io_channels[channel].val_offset == PWMA_VAL ? IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET : + IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET; + uint32_t rvalue = REG(timer, timer_io_channels[channel].sub_module, offset); + rvalue &= SMC_EDGA0_BOTH; + + switch (rvalue) { + + case (SMC_EDGA0_RISING): + *edge = Rising; + break; + + case (SMC_EDGA0_FALLING): + *edge = Falling; + break; + + case (SMC_EDGA0_BOTH): + *edge = Both; + 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 = 0; + + switch (edge) { + case Disabled: + break; + + case Rising: + edge_bits = SMC_EDGA0_RISING; + break; + + case Falling: + edge_bits = SMC_EDGA0_FALLING; + break; + + case Both: + edge_bits = SMC_EDGA0_BOTH; + break; + + default: + return -EINVAL;; + } + + uint32_t timer = timer_io_channels[channel].timer_index; + uint32_t offset = timer_io_channels[channel].val_offset == PWMA_VAL ? IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET : + IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET; + irqstate_t flags = px4_enter_critical_section(); + uint32_t rvalue = REG(timer, timer_io_channels[channel].sub_module, offset); + rvalue &= ~SMC_EDGA0_BOTH; + rvalue |= edge_bits; + REG(timer, timer_io_channels[channel].sub_module, offset) = rvalue; + px4_leave_critical_section(flags); + rv = OK; + } + } + + 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/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c new file mode 100644 index 00000000000..271d2e63386 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -0,0 +1,733 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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.c + * + * Servo driver supporting PWM servos connected to imxrt FLEXPWM blocks. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include "hardware/imxrt_flexpwm.h" +#include "imxrt_periphclks.h" + +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 1000000 +#endif + +#if !defined(BOARD_ONESHOT_FREQ) +#define BOARD_ONESHOT_FREQ 8000000 +#endif + +#define FLEXPWM_SRC_CLOCK_FREQ 16000000 + +#define MAX_CHANNELS_PER_TIMER 2 + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define REG(_tmr, _sm, _reg) _REG16(io_timers[(_tmr)].base + ((_sm) * SM_SPACING), (_reg)) + + +/* Timer register accessors */ + +#define rCNT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CNT_OFFSET) /* Counter Register */ +#define rINIT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ +#define rCTRL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */ +#define rCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */ +#define rVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */ +#define rFRACVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL1_OFFSET) /* Fractional Value Register 1 */ +#define rVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */ +#define rFRACVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL2_OFFSET) /* Fractional Value Register 2 */ +#define rVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */ +#define rFRACVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL3_OFFSET) /* Fractional Value Register 3 */ +#define rVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */ +#define rFRACVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL4_OFFSET) /* Fractional Value Register 4 */ +#define rVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */ +#define rFRACVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL5_OFFSET) /* Fractional Value Register 5 */ +#define rVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */ +#define rFRCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRCTRL_OFFSET) /* Fractional Control Register */ +#define rOCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0OCTRL_OFFSET) /* Output Control Register */ +#define rSTS(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0STS_OFFSET) /* Status Register */ +#define rINTEN(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INTEN_OFFSET) /* Interrupt Enable Register */ +#define rDMAEN(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DMAEN_OFFSET) /* DMA Enable Register */ +#define rTCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0TCTRL_OFFSET) /* Output Trigger Control Register */ +#define rDISMAP0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */ +#define rDISMAP1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */ +#define rDTCNT0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DTCNT0_OFFSET) /* Deadtime Count Register 0 */ +#define rDTCNT1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DTCNT1_OFFSET) /* Deadtime Count Register 1 */ +#define rCAPTCTRLA(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET) /* Capture Control A Register */ +#define rCAPTCOMPA(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPA_OFFSET) /* Capture Compare A Register */ +#define rCAPTCTRLB(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET) /* Capture Control B Register */ +#define rCAPTCOMPB(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPB_OFFSET) /* Capture Compare B Register */ +#define rCAPTCTRLX(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLX_OFFSET) /* Capture Control X Register */ +#define rCAPTCOMPX(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPX_OFFSET) /* Capture Compare X Register */ +#define rCVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL0_OFFSET) /* Capture Value 0 Register */ +#define rCVAL0CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL0CYC_OFFSET) /* Capture Value 0 Cycle Register */ +#define rCVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL1_OFFSET) /* Capture Value 1 Register */ +#define rCVAL1CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL1CYC_OFFSET) /* Capture Value 1 Cycle Register */ +#define rCVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL2_OFFSET) /* Capture Value 2 Register */ +#define rCVAL2CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL2CYC_OFFSET) /* Capture Value 2 Cycle Register */ +#define rCVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL3_OFFSET) /* Capture Value 3 Register */ +#define rCVAL3CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL3CYC_OFFSET) /* Capture Value 3 Cycle Register */ +#define rCVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL4_OFFSET) /* Capture Value 4 Register */ +#define rCVAL4CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL4CYC_OFFSET) /* Capture Value 4 Cycle Register */ +#define rCVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL5_OFFSET) /* Capture Value 5 Register */ +#define rCVAL5CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL5CYC_OFFSET) /* Capture Value 5 Cycle Register */ + +#define rOUTEN(_tim) REG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */ +#define rMASK(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MASK_OFFSET) /* Mask Register */ +#define rSWCOUT(_tim) REG(_tim, 0, IMXRT_FLEXPWM_SWCOUT_OFFSET) /* Software Controlled Output Register */ +#define rDTSRCSEL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ +#define rMCTRL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ +#define rMCTRL2(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL2_OFFSET) /* Master Control 2 Register */ +#define rFCTRL0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL0_OFFSET) /* Fault Control Register */ +#define rFSTS0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */ +#define rFFILT0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */ +#define rFTST0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FTST0_OFFSET) /* Fault Test Register */ +#define rFCTRL20(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL20_OFFSET) /* Fault Control 2 Register */ + + +// NotUsed PWMOut PWMIn Capture OneShot Trigger +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 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) +{ + // Not implemented yet + UNUSED(io_timer_channel_stats); + return 0; +} + +int io_timer_handler0(int irq, void *context, void *arg) +{ + + return io_timer_handler(0); +} + +int io_timer_handler1(int irq, void *context, void *arg) +{ + return io_timer_handler(1); + +} + +int io_timer_handler2(int irq, void *context, void *arg) +{ + return io_timer_handler(2); + +} + +int io_timer_handler3(int irq, void *context, void *arg) +{ + return io_timer_handler(3); + +} + +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 int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static uint32_t get_channel_mask(unsigned channel) +{ + return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0; +} + +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].val_offset != 0) { + + /* test timer for validity */ + + if (io_timers[channels_timer(channel)].base != 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 int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode, + io_timer_channel_mode_t new_mode) +{ + /* If caller mode is not based on current setting adjust it */ + + if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) { + mode = IOTimerChanMode_NotUsed; + } + + /* Remove old set of channels from original */ + + channel_allocations[mode] &= ~channels; + + /* Will this change ?*/ + + uint32_t before = channel_allocations[new_mode] & channels; + + /* add in the new set */ + + channel_allocations[new_mode] |= channels; + + /* Indicate a mode change */ + + return before ^ channels; +} + +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 channel, unsigned rate) +{ + irqstate_t flags = px4_enter_critical_section(); + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + px4_leave_critical_section(flags); + return 0; +} + +static inline void io_timer_set_oneshot_mode(unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~SMCTRL_PRSC_MASK; + rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + px4_leave_critical_section(flags); + +} + +static inline void io_timer_set_PWM_mode(unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); + rvalue |= SMCTRL_PRSC_DIV16; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + px4_leave_critical_section(flags); +} + +void io_timer_trigger(void) +{ + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + struct { + uint32_t base; + uint16_t triggers; + } action_cache[MAX_IO_TIMERS] = {0}; + int actions = 0; + + /* Pre-calculate the list of channels to Trigger */ + int mask; + + for (int timer = 0; timer < MAX_IO_TIMERS; timer++) { + action_cache[actions].base = io_timers[timer].base; + + if (action_cache[actions].base) { + for (uint32_t channel = io_timers[timer].first_channel_index; + channel <= io_timers[timer].last_channel_index; channel++) { + mask = get_channel_mask(channel); + + if (oneshots & mask) { + action_cache[actions].triggers |= timer_io_channels[channel].sub_module_bits; + } + } + + actions++; + } + } + + /* Now do them all with the shortest delay in between */ + + irqstate_t flags = px4_enter_critical_section(); + + for (actions = 0; action_cache[actions].base != 0 && actions < MAX_IO_TIMERS; actions++) { + _REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= (action_cache[actions].triggers >> MCTRL_LDOK_SHIFT) + << MCTRL_CLDOK_SHIFT ; + _REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= action_cache[actions].triggers; + } + + px4_leave_critical_section(flags); +} + +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 */ + + switch (io_timers[timer].base) { + case IMXRT_FLEXPWM1_BASE: + imxrt_clockall_pwm1(); + break; + + case IMXRT_FLEXPWM2_BASE: + imxrt_clockall_pwm2(); + break; + + case IMXRT_FLEXPWM3_BASE: + imxrt_clockall_pwm3(); + break; + + case IMXRT_FLEXPWM4_BASE: + imxrt_clockall_pwm4(); + break; + } + + for (uint32_t chan = io_timers[timer].first_channel_index; + chan <= io_timers[timer].last_channel_index; chan++) { + + /* Clear all Faults */ + rFSTS0(timer) = FSTS_FFLAG_MASK; + + rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; + rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; + rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) + : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); + rDTSRCSEL(timer) = 0; + rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); + rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; + io_timer_set_PWM_mode(chan); + timer_set_rate(chan, 50); + } + + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_rate(unsigned channel, unsigned rate) +{ + int rv = EBUSY; + + /* Get the channel bits that belong to the channel */ + + uint32_t channels = get_channel_mask(channel); + + /* Check that all channels are either in PWM or Oneshot */ + + if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | + channel_allocations[IOTimerChanMode_OneShot] | + channel_allocations[IOTimerChanMode_NotUsed])) == + channels) { + + /* Change only a timer that is owned by pwm or one shot */ + + /* Request to use OneShot ?*/ + + if (rate == 0) { + + /* Request to use OneShot + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be Oneshot + */ + + /* Did the allocation change */ + if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { + io_timer_set_oneshot_mode(channel); + } + + } else { + + /* Request to use PWM + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be PWM + */ + + if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { + io_timer_set_PWM_mode(channel); + } + + timer_set_rate(channel, rate); + } + + rv = OK; + } + + return rv; +} + +int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context) +{ + uint32_t gpio = 0; + + /* figure out the GPIO config first */ + + switch (mode) { + + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + gpio = timer_io_channels[channel].gpio_out; + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + return -EINVAL; + break; + + case IOTimerChanMode_NotUsed: + break; + + default: + return -EINVAL; + } + + int rv = allocate_channel(channel, mode); + + /* Valid channel should now be reserved in new mode */ + + if (rv >= 0) { + + unsigned timer = channels_timer(channel); + + /* Blindly try to initialize the timer - it will only do it once */ + + io_timer_init_timer(timer); + + irqstate_t flags = px4_enter_critical_section(); + + /* Set up IO */ + if (gpio) { + px4_arch_configgpio(gpio); + } + + /* configure the channel */ + + REG(timer, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) |= MCTRL_RUN(1 << timer_io_channels[channel].sub_module); + + channel_handlers[channel].callback = channel_handler; + channel_handlers[channel].context = context; + 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) +{ + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_OneShot: + case IOTimerChanMode_Trigger: + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + 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]; + + } + + struct { + uint32_t sm_ens; + uint32_t base; + uint32_t io_index; + uint32_t gpios[MAX_TIMER_IO_CHANNELS]; + } action_cache[MAX_IO_TIMERS]; + + memset(action_cache, 0, sizeof(action_cache)); + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + + if (io_timer_validate_channel_index(chan_index) == 0) { + uint32_t timer_index = channels_timer(chan_index); + action_cache[timer_index].base = io_timers[timer_index].base; + action_cache[timer_index].sm_ens |= MCTRL_RUN(1 << timer_io_channels[chan_index].sub_module) | + timer_io_channels[chan_index].sub_module_bits; + action_cache[timer_index].gpios[action_cache[timer_index].io_index++] = timer_io_channels[chan_index].gpio_out; + } + } + } + + irqstate_t flags = px4_enter_critical_section(); + + for (unsigned actions = 0; actions < arraySize(action_cache); actions++) { + if (action_cache[actions].base != 0) { + for (unsigned int index = 0; index < action_cache[actions].io_index; index++) { + if (action_cache[actions].gpios[index]) { + px4_arch_configgpio(action_cache[actions].gpios[index]); + } + + _REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) = action_cache[actions].sm_ens; + } + } + } + + 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); + int mode = io_timer_get_channel_mode(channel); + + if (rv == 0) { + if ((mode != IOTimerChanMode_PWMOut) && + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Trigger)) { + + rv = -EIO; + + } else { + irqstate_t flags = px4_enter_critical_section(); + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + REG(channels_timer(channel), timer_io_channels[channel].sub_module, timer_io_channels[channel].val_offset) = value - 1; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + px4_leave_critical_section(flags); + } + } + + return rv; +} + +uint16_t io_channel_get_ccr(unsigned channel) +{ + uint16_t value = 0; + + if (io_timer_validate_channel_index(channel) == 0) { + int mode = io_timer_get_channel_mode(channel); + + if ((mode == IOTimerChanMode_PWMOut) || + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { + value = REG(channels_timer(channel), timer_io_channels[channel].sub_module, timer_io_channels[channel].val_offset) + 1; + } + } + + return value; +} + +// The rt has 1:1 group to channel +uint32_t io_timer_get_group(unsigned group) +{ + return get_channel_mask(group); +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c new file mode 100644 index 00000000000..3af467fac3e --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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 FLexPWM timer blocks. + * N.B. Groups:channels have a 1:1 correspondence on FlexPWM + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +//#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 channel, unsigned rate) +{ + if (io_timer_validate_channel_index(channel) < 0) { + return ERROR; + } + + /* Allow a rate of 0 to enter oneshot mode */ + + if (rate != 0) { + + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + + if (rate < 1) { + return -ERANGE; + } + + if (rate > 10000) { + return -ERANGE; + } + } + + return io_timer_set_rate(channel, rate); +} + +void up_pwm_update(void) +{ + io_timer_trigger(); +} + +int up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; 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_OneShot, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c new file mode 100644 index 00000000000..7e9869d0d3e --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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_trigger.c + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +int up_pwm_trigger_set(unsigned channel, uint16_t value) +{ + return io_timer_set_ccr(channel, value); +} + +int up_pwm_trigger_init(uint32_t channel_mask) +{ + /* Init channels */ + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not trigger mode before + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + /* Enable the timers */ + up_pwm_trigger_arm(true); + + return OK; +} + +void up_pwm_trigger_deinit() +{ + /* Disable the timers */ + up_pwm_trigger_arm(false); + + /* Deinit channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_Trigger); + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + + io_timer_channel_init(channel, IOTimerChanMode_NotUsed, NULL, NULL); + current &= ~(1 << channel); + } + } +} + +void +up_pwm_trigger_arm(bool armed) +{ + io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/CMakeLists.txt new file mode 100644 index 00000000000..1b150435261 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_led_pwm + led_pwm.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp new file mode 100644 index 00000000000..71ec0105956 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp @@ -0,0 +1,354 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 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 +#include "hardware/imxrt_tmr.h" + +int led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + return 0; +} +int led_pwm_servo_init(void) +{ + return 0; + +} + +#if 0 && defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM) + +#define FTM_SRC_CLOCK_FREQ 16000000 +#define LED_PWM_FREQ 1000000 + +#if (BOARD_LED_PWM_RATE) +# define LED_PWM_RATE BOARD_LED_PWM_RATE +#else +# define LED_PWM_RATE 50 +#endif + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(led_pwm_timers[_tmr].base, _reg) + + +/* Timer register accessors */ + +#define rSC(_tmr) REG(_tmr,KINETIS_FTM_SC_OFFSET) +#define rCNT(_tmr) REG(_tmr,KINETIS_FTM_CNT_OFFSET) +#define rMOD(_tmr) REG(_tmr,KINETIS_FTM_MOD_OFFSET) +#define rC0SC(_tmr) REG(_tmr,KINETIS_FTM_C0SC_OFFSET) +#define rC0V(_tmr) REG(_tmr,KINETIS_FTM_C0V_OFFSET) +#define rC1SC(_tmr) REG(_tmr,KINETIS_FTM_C1SC_OFFSET) +#define rC1V(_tmr) REG(_tmr,KINETIS_FTM_C1V_OFFSET) +#define rC2SC(_tmr) REG(_tmr,KINETIS_FTM_C2SC_OFFSET) +#define rC2V(_tmr) REG(_tmr,KINETIS_FTM_C2V_OFFSET) +#define rC3SC(_tmr) REG(_tmr,KINETIS_FTM_C3SC_OFFSET) +#define rC3V(_tmr) REG(_tmr,KINETIS_FTM_C3V_OFFSET) +#define rC4SC(_tmr) REG(_tmr,KINETIS_FTM_C4SC_OFFSET) +#define rC4V(_tmr) REG(_tmr,KINETIS_FTM_C4V_OFFSET) +#define rC5SC(_tmr) REG(_tmr,KINETIS_FTM_C5SC_OFFSET) +#define rC5V(_tmr) REG(_tmr,KINETIS_FTM_C5V_OFFSET) +#define rC6SC(_tmr) REG(_tmr,KINETIS_FTM_C6SC_OFFSET) +#define rC6V(_tmr) REG(_tmr,KINETIS_FTM_C6V_OFFSET) +#define rC7SC(_tmr) REG(_tmr,KINETIS_FTM_C7SC_OFFSET) +#define rC7V(_tmr) REG(_tmr,KINETIS_FTM_C7V_OFFSET) + +#define rCNTIN(_tmr) REG(_tmr,KINETIS_FTM_CNTIN_OFFSET) +#define rSTATUS(_tmr) REG(_tmr,KINETIS_FTM_STATUS_OFFSET) +#define rMODE(_tmr) REG(_tmr,KINETIS_FTM_MODE_OFFSET) +#define rSYNC(_tmr) REG(_tmr,KINETIS_FTM_SYNC_OFFSET) +#define rOUTINIT(_tmr) REG(_tmr,KINETIS_FTM_OUTINIT_OFFSET) +#define rOUTMASK(_tmr) REG(_tmr,KINETIS_FTM_OUTMASK_OFFSET) +#define rCOMBINE(_tmr) REG(_tmr,KINETIS_FTM_COMBINE_OFFSET) +#define rDEADTIME(_tmr) REG(_tmr,KINETIS_FTM_DEADTIME_OFFSET) +#define rEXTTRIG(_tmr) REG(_tmr,KINETIS_FTM_EXTTRIG_OFFSET) +#define rPOL(_tmr) REG(_tmr,KINETIS_FTM_POL_OFFSET) +#define rFMS(_tmr) REG(_tmr,KINETIS_FTM_FMS_OFFSET) +#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET) +#define rFLTCTRL(_tmr) REG(_tmr,KINETIS_FTM_FLTCTRL_OFFSET) +#define rQDCTRL(_tmr) REG(_tmr,KINETIS_FTM_QDCTRL_OFFSET) +#define rCONF(_tmr) REG(_tmr,KINETIS_FTM_CONF_OFFSET) +#define rFLTPOL(_tmr) REG(_tmr,KINETIS_FTM_FLTPOL_OFFSET) +#define rSYNCONF(_tmr) REG(_tmr,KINETIS_FTM_SYNCONF_OFFSET) +#define rINVCTRL(_tmr) REG(_tmr,KINETIS_FTM_INVCTRL_OFFSET) +#define rSWOCTRL(_tmr) REG(_tmr,KINETIS_FTM_SWOCTRL_OFFSET) +#define rPWMLOAD(_tmr) REG(_tmr,KINETIS_FTM_PWMLOAD_OFFSET) + +#define CnSC_RESET (FTM_CSC_CHF|FTM_CSC_CHIE|FTM_CSC_MSB|FTM_CSC_MSA|FTM_CSC_ELSB|FTM_CSC_ELSA|FTM_CSC_DMA) +#define CnSC_CAPTURE_INIT (FTM_CSC_CHIE|FTM_CSC_ELSB|FTM_CSC_ELSA) // Both + +#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) +#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSA) +#else +#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSB) +#endif + +#define FTM_SYNC (FTM_SYNC_SWSYNC) + +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_set_rate(unsigned timer, unsigned rate) +{ + + irqstate_t flags = px4_enter_critical_section(); + + uint32_t save = rSC(timer); + rSC(timer) = save & ~(FTM_SC_CLKS_MASK); + + /* configure the timer to update at the desired rate */ + rMOD(timer) = (LED_PWM_FREQ / rate) - 1; + rSC(timer) = save; + + px4_leave_critical_section(flags); +} + +static inline uint32_t div2psc(int div) +{ + return 31 - __builtin_clz(div); +} + +static inline void led_pwm_timer_set_PWM_mode(unsigned timer) +{ + irqstate_t flags = px4_enter_critical_section(); + rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK); + rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / LED_PWM_FREQ)); + px4_leave_critical_section(flags); +} + + +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 */ + + uint32_t regval = _REG(led_pwm_timers[timer].clock_register); + regval |= led_pwm_timers[timer].clock_bit; + _REG(led_pwm_timers[timer].clock_register) = regval; + + /* disable and configure the timer */ + + rSC(timer) = FTM_SC_CLKS_NONE; + rCNT(timer) = 0; + + rMODE(timer) = 0; + rSYNCONF(timer) = (FTM_SYNCONF_SYNCMODE | FTM_SYNCONF_SWWRBUF | FTM_SYNCONF_SWRSTCNT); + + /* Set to run in debug mode */ + + rCONF(timer) |= FTM_CONF_BDMMODE_MASK; + + /* enable the timer */ + + led_pwm_timer_set_PWM_mode(timer); + + /* + * Note we do the Standard PWM Out init here + * default to updating at LED_PWM_RATE + */ + + led_pwm_timer_set_rate(timer, LED_PWM_RATE); + } +} +unsigned +led_pwm_timer_get_period(unsigned timer) +{ + // MOD is a 16 bit reg + unsigned mod = rMOD(timer); + + if (mod == 0) { + return 1 << 16; + } + + return (uint16_t)(mod + 1); +} + + +static void +led_pwm_channel_init(unsigned channel) +{ + /* Only initialize used channels */ + + if (led_pwm_channels[channel].timer_channel) { + unsigned timer = led_pwm_channels[channel].timer_index; + + irqstate_t flags = px4_enter_critical_section(); + + /* configure the GPIO first */ + + px4_arch_configgpio(led_pwm_channels[channel].gpio_out); + + /* configure the channel */ + + uint32_t chan = led_pwm_channels[channel].timer_channel - 1; + + uint16_t rvalue = REG(timer, KINETIS_FTM_CSC_OFFSET(chan)); + rvalue &= ~CnSC_RESET; + rvalue |= CnSC_PWMOUT_INIT; + REG(timer, KINETIS_FTM_CSC_OFFSET(chan)) = rvalue; + REG(timer, KINETIS_FTM_CV_OFFSET(0)) = 0; + px4_leave_critical_section(flags); + } +} + +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); + + unsigned value = (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1)) = value; + + 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 value; + } + + value = REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1)); + 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 */ + led_pwm_timer_set_PWM_mode(i); + + } else { + /* disable and configure the timer */ + + rSC(i) = FTM_SC_CLKS_NONE; + rCNT(i) = 0; + } + } + } +} + +#endif // BOARD_HAS_LED_PWM || BOARD_HAS_UI_LED_PWM diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/CMakeLists.txt new file mode 100644 index 00000000000..a2d2f8144bc --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_tone_alarm + ToneAlarmInterface.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp new file mode 100644 index 00000000000..b394ea88ea6 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * Copyright (C) 2013-2019 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 ToneAlarmInterface.cpp + */ + +#include +#include +#include +#include + +#include "chip.h" +#include "hardware/imxrt_gpt.h" +#include "imxrt_periphclks.h" + +#define CAT3_(A, B, C) A##B##C +#define CAT3(A, B, C) CAT3_(A, B, C) + +#define CAT2_(A, B) A##B +#define CAT2(A, B) CAT2_(A, B) + +/* 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 + +/* +* Period of the free-running counter, in microseconds. +*/ +#define TONE_ALARM_COUNTER_PERIOD 4294967296 + +/* Tone Alarm configuration */ + +#define TONE_ALARM_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */ +#define TONE_ALARM_TIMER_BASE CAT3(IMXRT_GPT, TONE_ALARM_TIMER,_BASE) /* The Base address of the GPT */ +#define TONE_ALARM_TIMER_VECTOR CAT(IMXRT_IRQ_GPT, TONE_ALARM_TIMER) /* The GPT Interrupt vector */ + +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */ +#elif TONE_ALARM_TIMER == 2 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */ +#endif + +#if TONE_ALARM_TIMER == 1 && defined(CONFIG_IMXRT_GPT1) +# error must not set CONFIG_IMXRT_GPT1=y and TONE_ALARM_TIMER=1 +#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_IMXRT_GPT2) +# error must not set CONFIG_IMXRT_GPT2=y and TONE_ALARM_TIMER=2 +#endif + + +# define TONE_ALARM_TIMER_FREQ 1000000 + +/* +* Tone Alarm clock must be a multiple of 1MHz greater than 1MHz +*/ +#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_TIMER_FREQ) != 0 +# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_TIMER_FREQ +# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz +#endif + +#if (TONE_ALARM_TIMER_CHANNEL > 1) || (TONE_ALARM_TIMER_CHANNEL > 3) +# error TONE_ALARM_CHANNEL must be a value between 1 and 3 +#endif + + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* Timer register accessors */ + +#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg)) + +#define rCR REG(IMXRT_GPT_CR_OFFSET) +#define rPR REG(IMXRT_GPT_PR_OFFSET) +#define rSR REG(IMXRT_GPT_SR_OFFSET) +#define rIR REG(IMXRT_GPT_IR_OFFSET) +#define rOCR1 REG(IMXRT_GPT_OCR1_OFFSET) +#define rOCR2 REG(IMXRT_GPT_OCR2_OFFSET) +#define rOCR3 REG(IMXRT_GPT_OCR3_OFFSET) +#define rICR1 REG(IMXRT_GPT_ICR1_OFFSET) +#define rICR2 REG(IMXRT_GPT_ICR2_OFFSET) +#define rCNT REG(IMXRT_GPT_CNT_OFFSET) + +/* +* Specific registers and bits used by Tone Alarm sub-functions +*/ + +#define rOCR CAT2(rOCR, TONE_ALARM_CHANNEL) /* GPT Output Compare Register used by HRT */ +#define rSTATUS CAT2(GPT_SR_OF, TONE_ALARM_CHANNEL) /* OF Output Compare Flag */ +#define CR_OM CAT3(GPT_CR_OM, TONE_ALARM_CHANNEL,_TOGGLE) /* Output Compare mode */ + + +#define CBRK_BUZZER_KEY 782097 + +namespace ToneAlarmInterface +{ + +void init() +{ +#if defined(TONE_ALARM_TIMER) + /* configure the GPIO to the idle state */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); + + /* Enable the Module clock */ + + TONE_ALARM_CLOCK_ALL(); + + rCR = 0; + + + /* disable and configure the timer */ + + /* Use Restart mode. The FFR bit determines the behavior of the GPT + * when a compare event in _channel 1_ occurs. + */ + rCR = GPT_CR_ENMOD | GPT_CR_DBGEN | GPT_CR_WAITEN | + GPT_CR_IM2_DIS | GPT_CR_IM1_DIS | + CR_OM | GPT_CR_CLKSRC_IPG; + + /* CLKSRC field is divided by [PRESCALER + 1] */ + + rPR = (TONE_ALARM_TIMER_CLOCK / TONE_ALARM_TIMER_FREQ) - 1; + + /* enable the timer and output toggle */ + + rCR |= GPT_CR_EN; +#endif /* TONE_ALARM_TIMER */ +} + +void start_note(unsigned frequency) +{ +#if defined(TONE_ALARM_TIMER) + float period = 0.5f / frequency; + + // and the divisor, rounded to the nearest integer + unsigned divisor = (period * TONE_ALARM_TIMER_FREQ) + 0.5f; + + rCR &= ~GPT_CR_EN; + rOCR = divisor; // load new toggle period + + /* We must always load and run Channel 1 in parallel + * with the Timer out channel used to drive the tone. + * The will to initiate the reset in Restart mode. + */ + rOCR1 = divisor; + rCR |= GPT_CR_EN; + + // configure the GPIO to enable timer output + px4_arch_configgpio(GPIO_TONE_ALARM); +#endif /* TONE_ALARM_TIMER */ +} + +void stop_note() +{ +#if defined(TONE_ALARM_TIMER) + /* stop the current note */ + + rCR &= ~GPT_CR_EN; + + /* + * Make sure the GPIO is not driving the speaker. + */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +#endif /* TONE_ALARM_TIMER */ +} + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/nxp/imxrt/version/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/version/CMakeLists.txt new file mode 100644 index 00000000000..3303bdea437 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/version/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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_library(arch_version + board_identity.c + board_mcu_version.c +) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c new file mode 100644 index 00000000000..f1ba79a67ef --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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_identity.c + * Implementation of imxrt based Board identity API + */ + +#include +#include +#include +#include +#include +#include + +#define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4} +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID; + +/* A type suitable for holding the reordering array for the byte format of the UUID + */ + +typedef const uint8_t uuid_uint8_reorder_t[PX4_CPU_UUID_BYTE_LENGTH]; + +void board_get_uuid(uuid_byte_t uuid_bytes) +{ + uuid_uint8_reorder_t reorder = CPU_UUID_BYTE_FORMAT_ORDER; + + union { + uuid_byte_t b; + uuid_uint32_t w; + } id; + + /* Copy the serial from the OCOTP */ + + board_get_uuid32(id.w); + + /* swap endianess */ + + for (int i = 0; i < PX4_CPU_UUID_BYTE_LENGTH; i++) { + uuid_bytes[i] = id.b[reorder[i]]; + } +} + +void board_get_uuid32(uuid_uint32_t uuid_words) +{ + /* IMXRT_OCOTP_CFG1:0x420[10:0], IMXRT_OCOTP_CFG0:0x410[31:0] LOT_NO_ENC[42:0](SJC_CHALL/UNIQUE_ID[42:0]) + * 43 bits FSL-wide unique,encoded LOT ID STD II/SJC CHALLENGE/ Unique ID + * 0x420[15:11] WAFER_NO[4:0]( SJC_CHALL[47:43] /UNIQUE_ID[47:43]) + * 5 bits The wafer number of the wafer on which the device was fabricated/SJC CHALLENGE/ Unique ID + * 0x420[23:16] DIE-YCORDINATE[7:0]( SJC_CHALL[55:48] /UNIQUE_ID[55:48]) + * 8 bits The Y-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID + * 0x420[31:24] DIE-XCORDINATE[7:0]( SJC_CHALL[63:56] /UNIQUE_ID[63:56] ) + * 8 bits The X-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID + * + * word [0] word[1] + * SJC_CHALL[63:32] [31:00] + */ + + uuid_words[0] = getreg32(IMXRT_OCOTP_CFG1); + uuid_words[1] = getreg32(IMXRT_OCOTP_CFG0); +} + +int board_get_uuid32_formated(char *format_buffer, int size, + const char *format, + const char *seperator) +{ + uuid_uint32_t uuid; + board_get_uuid32(uuid); + + int offset = 0; + int sep_size = seperator ? strlen(seperator) : 0; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]); + + if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) { + strcat(&format_buffer[offset], seperator); + offset += sep_size; + } + } + + return 0; +} + +int board_get_mfguid(mfguid_t mfgid) +{ + board_get_uuid(* (uuid_byte_t *) mfgid); + return PX4_CPU_MFGUID_BYTE_LENGTH; +} + +int board_get_mfguid_formated(char *format_buffer, int size) +{ + mfguid_t mfguid; + + board_get_mfguid(mfguid); + int offset = 0; + + for (unsigned int i = 0; i < PX4_CPU_MFGUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", mfguid[i]); + } + + return offset; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + for (unsigned i = 0; i < PX4_GUID_BYTE_LENGTH - (sizeof(soc_arch_id) + PX4_CPU_UUID_BYTE_LENGTH); i++) { + *pb++ = 0; + } + + board_get_uuid(pb); + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/version/board_mcu_version.c b/platforms/nuttx/src/px4/nxp/imxrt/version/board_mcu_version.c new file mode 100644 index 00000000000..cad6c437536 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/version/board_mcu_version.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2018, 2019 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_mcu_version.c + * Implementation of imxrt based SoC version API + */ + +#include +#include + +#include +#include +#include "up_arch.h" + +#define DIGPROG_MINOR_SHIFT 0 +#define DIGPROG_MINOR_MASK (0xff << DIGPROG_MINOR_SHIFT) +#define DIGPROG_MINOR(info) (((info) & DIGPROG_MINOR_MASK) >> DIGPROG_MINOR_SHIFT) +#define DIGPROG_MAJOR_LOWER_SHIFT 8 +#define DIGPROG_MAJOR_LOWER_MASK (0xff << DIGPROG_MAJOR_LOWER_SHIFT) +#define DIGPROG_MAJOR_LOWER(info) (((info) & DIGPROG_MAJOR_LOWER_MASK) >> DIGPROG_MAJOR_LOWER_SHIFT) +#define DIGPROG_MAJOR_UPPER_SHIFT 16 +#define DIGPROG_MAJOR_UPPER_MASK (0xff << DIGPROG_MAJOR_UPPER_SHIFT) +#define DIGPROG_MAJOR_UPPER(info) (((info) & DIGPROG_MAJOR_UPPER_MASK) >> DIGPROG_MAJOR_UPPER_SHIFT) +// 876543210 +#define CHIP_TAG "i.MX RT10?2 r?.?" +#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1 + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + uint32_t info = getreg32(IMXRT_USB_ANALOG_DIGPROG); + static char chip[sizeof(CHIP_TAG)] = CHIP_TAG; + + chip[CHIP_TAG_LEN - 1] = '0' + DIGPROG_MINOR(info); + chip[CHIP_TAG_LEN - 3] = '1' + DIGPROG_MAJOR_LOWER(info); + chip[CHIP_TAG_LEN - 7] = DIGPROG_MAJOR_UPPER(info) == 0x6a ? '5' : '6'; + *revstr = chip; + *rev = '0' + DIGPROG_MINOR(info); + + if (errata) { + *errata = NULL; + } + + return 0; +} diff --git a/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt new file mode 100644 index 00000000000..1bb46354db7 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + + +add_subdirectory(../imxrt/adc adc) +add_subdirectory(../imxrt/board_critmon board_critmon) +add_subdirectory(../imxrt/board_hw_info board_hw_info) +add_subdirectory(../imxrt/board_reset board_reset) +#add_subdirectory(../imxrt/dshot dshot) +add_subdirectory(../imxrt/hrt hrt) +add_subdirectory(../imxrt/led_pwm led_pwm) +add_subdirectory(../imxrt/io_pins io_pins) +add_subdirectory(../imxrt/tone_alarm tone_alarm) +add_subdirectory(../imxrt/version version) diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/adc.h new file mode 100644 index 00000000000..f9da7fb8f5d --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/adc.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../imxrt/include/px4_arch/adc.h" diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h new file mode 100644 index 00000000000..328ac3eddfc --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ +#pragma once + + +#include "../../../imxrt/include/px4_arch/io_timer.h" diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h new file mode 100644 index 00000000000..311d8490e08 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ +#pragma once + + +#include + +__BEGIN_DECLS + +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_NXPIMXRT1062 + +#// Fixme: using ?? +#define PX4_BBSRAM_SIZE 2048 +#define PX4_BBSRAM_GETDESC_IOCTL 0 +#define PX4_NUMBER_I2C_BUSES 4 + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +//# include todo:Upsteam UID access + +/* imxrt defines the 64 bit UUID as + * + * OCOTP 0x410 bits 31:0 + * OCOTP 0x420 bits 63:32 + * + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] + * bits 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH 8 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* define common formating across all commands */ + +#define PX4_CPU_UUID_WORD32_FORMAT "%08x" +#define PX4_CPU_UUID_WORD32_SEPARATOR ":" + +#define PX4_CPU_UUID_WORD32_UNIQUE_H 0 /* Least significant digits change the most (die wafer,X,Y */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Most significant digits change the least (lot#) */ + +/* Separator nnn:nnn:nnnn 2 char per byte term */ +#define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) +#define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) + +#define imxrt_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet + +#define px4_savepanic(fileno, context, length) imxrt_bbsram_savepanic(fileno, context, length) + +/* bus_num is 1 based on imx and must be translated from the legacy one based */ + +#define PX4_BUS_OFFSET 0 /* imxrt buses are 1 based no adjustment needed */ + +#define px4_spibus_initialize(bus_num_1based) imxrt_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) imxrt_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) imxrt_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) imxrt_config_gpio(pinset) +#define px4_arch_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) imxrt_gpio_read(pinset) +#define px4_arch_gpiowrite(pinset, value) imxrt_gpio_write(pinset, value) + +/* imxrt_gpiosetevent is not implemented and will need to be added */ + +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) imxrt_gpiosetevent(pinset,r,f,e,fp,a) + + +__END_DECLS