From 490fe8256f406f383d105ad0c24691f8979e7f46 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 4 Dec 2019 10:58:07 -0800 Subject: [PATCH] Add nxp imxrt device support --- .../px4_platform_common/board_common.h | 1 + platforms/nuttx/CMakeLists.txt | 5 +- platforms/nuttx/cmake/px4_impl_os.cmake | 3 + .../src/px4/nxp/imxrt/adc/CMakeLists.txt | 36 + platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp | 197 ++++ .../nxp/imxrt/board_critmon/CMakeLists.txt | 36 + .../nxp/imxrt/board_critmon/board_critmon.c | 66 ++ .../nxp/imxrt/board_hw_info/CMakeLists.txt | 36 + .../imxrt/board_hw_info/board_hw_rev_ver.c | 350 +++++++ .../px4/nxp/imxrt/board_reset/CMakeLists.txt | 36 + .../px4/nxp/imxrt/board_reset/board_reset.c | 91 ++ .../src/px4/nxp/imxrt/hrt/CMakeLists.txt | 37 + platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c | 875 ++++++++++++++++++ .../src/px4/nxp/imxrt/include/px4_arch/adc.h | 45 + .../px4/nxp/imxrt/include/px4_arch/io_timer.h | 142 +++ .../src/px4/nxp/imxrt/io_pins/CMakeLists.txt | 39 + .../src/px4/nxp/imxrt/io_pins/input_capture.c | 330 +++++++ .../src/px4/nxp/imxrt/io_pins/io_timer.c | 733 +++++++++++++++ .../src/px4/nxp/imxrt/io_pins/pwm_servo.c | 161 ++++ .../src/px4/nxp/imxrt/io_pins/pwm_trigger.c | 107 +++ .../src/px4/nxp/imxrt/led_pwm/CMakeLists.txt | 36 + .../src/px4/nxp/imxrt/led_pwm/led_pwm.cpp | 354 +++++++ .../px4/nxp/imxrt/tone_alarm/CMakeLists.txt | 36 + .../imxrt/tone_alarm/ToneAlarmInterface.cpp | 193 ++++ .../src/px4/nxp/imxrt/version/CMakeLists.txt | 37 + .../px4/nxp/imxrt/version/board_identity.c | 170 ++++ .../px4/nxp/imxrt/version/board_mcu_version.c | 76 ++ .../nuttx/src/px4/nxp/rt106x/CMakeLists.txt | 44 + .../src/px4/nxp/rt106x/include/px4_arch/adc.h | 35 + .../nxp/rt106x/include/px4_arch/io_timer.h | 36 + .../nxp/rt106x/include/px4_arch/micro_hal.h | 109 +++ 31 files changed, 4451 insertions(+), 1 deletion(-) create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/adc/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_critmon/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/hrt/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/io_pins/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/led_pwm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/version/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/version/board_mcu_version.c create mode 100644 platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 9d87fcf9bd..3050deecd4 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 720b589d5c..acb9fa0b7f 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/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 8a601eb6f0..31c45cd4e4 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 0000000000..d2487d05bf --- /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 0000000000..ec601d8882 --- /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 0000000000..26cdb4bbc2 --- /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 0000000000..14e7a3bb3b --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c @@ -0,0 +1,66 @@ +/************************************************************************************ + * + * 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 + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "missing implementation for up_critmon_gettime() and up_critmon_convert()" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +// uint32_t up_critmon_gettime(void) +// { +// } + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +// void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +// { +// } + +#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 0000000000..3a466715b1 --- /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 0000000000..6270db805a --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -0,0 +1,350 @@ +/**************************************************************************** + * + * 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 defined(ON_EVK) + + if (1 || high || low) { // no if +#else + if ((high ^ low) && low == 0) { +#endif + /* 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 0000000000..e818a47336 --- /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 0000000000..1175c42df6 --- /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 0000000000..3edd775b6a --- /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 0000000000..25eef08265 --- /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 5 +#define HRT_INTERVAL_MAX 4294000000 + +/* +* 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 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + hrtinfo("due soon\n"); + deadline = next->deadline; + } + } + + hrtinfo("schedule for %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 0000000000..b2b4c60967 --- /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 0000000000..d8ebd7c9ef --- /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 0000000000..401bf06ef5 --- /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 0000000000..a5b0e3ce4c --- /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 0000000000..271d2e6338 --- /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 0000000000..3af467fac3 --- /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 0000000000..7e9869d0d3 --- /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 0000000000..1b15043526 --- /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 0000000000..71ec010595 --- /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 0000000000..a2d2f8144b --- /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 0000000000..d4fbe40b85 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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(); + + + /* disable and configure the timer */ + + /* disable and configure the timer */ + + rCR = GPT_CR_OM1_DIS | GPT_CR_OM2_DIS | GPT_CR_OM3_DIS | + CR_OM | GPT_CR_FRR | 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 + 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 0000000000..3303bdea43 --- /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 0000000000..f1ba79a67e --- /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 0000000000..cad6c43753 --- /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 0000000000..1bb46354db --- /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 0000000000..f9da7fb8f5 --- /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 0000000000..328ac3eddf --- /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 0000000000..311d8490e0 --- /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