From d56a5c97680a132a5dc8b1e277651d6d1924cb5d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 1 Feb 2020 14:57:39 -0800 Subject: [PATCH 1/4] NuttX master with NXP contrib Kinetis, imxrt, s32k, droneboards --- platforms/nuttx/NuttX/apps | 2 +- platforms/nuttx/NuttX/nuttx | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index 91b6ad6a0d00..960a92c652af 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit 91b6ad6a0d00ac107088b18588cd54d40e3cb796 +Subproject commit 960a92c652afc3c3c6deec63af3e7ca659db81d6 diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 7b36108b5486..5e9ad0588920 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 7b36108b548642f1a8513dcf00eb4913f8558ccf +Subproject commit 5e9ad0588920781822aeedbd266fe40a22e3602b From 4e81a519c4d9e2b2bd266b709be0879cfe306710 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 1 Feb 2020 14:55:39 -0800 Subject: [PATCH 2/4] s32k1xx:Add stubbed out chip and arch --- .../px4_platform_common/board_common.h | 2 + platforms/nuttx/cmake/px4_impl_os.cmake | 3 + .../nuttx/src/px4/nxp/s32k14x/CMakeLists.txt | 42 + .../px4/nxp/s32k14x/include/px4_arch/adc.h | 35 + .../nxp/s32k14x/include/px4_arch/io_timer.h | 35 + .../nxp/s32k14x/include/px4_arch/micro_hal.h | 114 +++ .../src/px4/nxp/s32k1xx/adc/CMakeLists.txt | 36 + .../nuttx/src/px4/nxp/s32k1xx/adc/adc.cpp | 190 ++++ .../nxp/s32k1xx/board_critmon/CMakeLists.txt | 36 + .../nxp/s32k1xx/board_critmon/board_critmon.c | 66 ++ .../nxp/s32k1xx/board_reset/CMakeLists.txt | 36 + .../px4/nxp/s32k1xx/board_reset/board_reset.c | 109 ++ .../src/px4/nxp/s32k1xx/hrt/CMakeLists.txt | 37 + platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c | 936 ++++++++++++++++++ .../px4/nxp/s32k1xx/include/px4_arch/adc.h | 39 + .../nxp/s32k1xx/include/px4_arch/io_timer.h | 137 +++ .../px4/nxp/s32k1xx/io_pins/CMakeLists.txt | 39 + .../px4/nxp/s32k1xx/io_pins/input_capture.c | 455 +++++++++ .../src/px4/nxp/s32k1xx/io_pins/io_timer.c | 891 +++++++++++++++++ .../src/px4/nxp/s32k1xx/io_pins/pwm_servo.c | 162 +++ .../src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c | 107 ++ .../px4/nxp/s32k1xx/led_pwm/CMakeLists.txt | 36 + .../src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp | 345 +++++++ .../px4/nxp/s32k1xx/tone_alarm/CMakeLists.txt | 36 + .../s32k1xx/tone_alarm/ToneAlarmInterface.cpp | 188 ++++ .../px4/nxp/s32k1xx/version/CMakeLists.txt | 37 + .../px4/nxp/s32k1xx/version/board_identity.c | 136 +++ .../nxp/s32k1xx/version/board_mcu_version.c | 66 ++ 28 files changed, 4351 insertions(+) create mode 100644 platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/adc/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/adc/adc.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/board_critmon/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/board_critmon/board_critmon.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/hrt/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/version/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k1xx/version/board_mcu_version.c diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 3050deecd4e6..30638503c099 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -384,6 +384,8 @@ typedef enum PX4_SOC_ARCH_ID_t { PX4_SOC_ARCH_ID_STM32H7 = 0x0006, + PX4_SOC_ARCH_ID_NXPS32K146 = 0x0007, + PX4_SOC_ARCH_ID_EAGLE = 0x1001, PX4_SOC_ARCH_ID_QURT = 0x1002, PX4_SOC_ARCH_ID_OCPOC = 0x1003, diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 31c45cd4e493..67ce2e770077 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -121,6 +121,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) set(CHIP_MANUFACTURER "nxp") set(CHIP "rt106x") + elseif(CONFIG_ARCH_CHIP_S32K146) + set(CHIP_MANUFACTURER "nxp") + set(CHIP "s32k14x") 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/s32k14x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt new file mode 100644 index 000000000000..02bc4d920d70 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(../s32k1xx/adc adc) +add_subdirectory(../s32k1xx/board_reset board_reset) +add_subdirectory(../s32k1xx/board_critmon board_critmon) +add_subdirectory(../s32k1xx/led_pwm led_pwm) +add_subdirectory(../s32k1xx/hrt hrt) +add_subdirectory(../s32k1xx/io_pins io_pins) +add_subdirectory(../s32k1xx/tone_alarm tone_alarm) +add_subdirectory(../s32k1xx/version version) diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/adc.h new file mode 100644 index 000000000000..1a1f7b7e15b1 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/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 "../../../s32k1xx/include/px4_arch/adc.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..260cd6614c55 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/io_timer.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 "../../../s32k1xx/include/px4_arch/io_timer.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..38e9106f698e --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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_NXPS32K146 + +// Fixme: using ?? +#define PX4_BBSRAM_SIZE 2048 +#define PX4_BBSRAM_GETDESC_IOCTL 0 +#define PX4_NUMBER_I2C_BUSES 2 + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +#include +//#include + +/* s32k1xx defines the 128 bit UUID as + * init32_t[4] that can be read as words + * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 127:96 (offset 0) + * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 95:64 (offset 4) + * init32_t[2] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 8) + * init32_t[3] PX4_CPU_UUID_ADDRESS[3] bits 31:0 (offset C) + * + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] [2] [3] + * bits 127:96 95-64 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#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 3 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ + +/* 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 s32k1xx_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet + +#define px4_savepanic(fileno, context, length) s32k1xx_bbsram_savepanic(fileno, context, length) + +/* bus_num is zero based on s32k1xx and must be translated from the legacy one based */ + +#define PX4_BUS_OFFSET 1 /* s32k1xx buses are 0 based and adjustment is needed */ + +#define px4_spibus_initialize(bus_num_1based) s32k1xx_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) s32k1xx_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) s32k1xx_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) s32k1xx_pinconfig(pinset) +#define px4_arch_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) s32k1xx_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) s32k1xx_gpiowrite(pinset, value) + +/* s32k1xx_gpiosetevent is not implemented and will need to be added */ + +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) s32k1xx_gpiosetevent(pinset,r,f,e,fp,a) + +#define I2C_RESET(q) +__END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/adc/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/adc/CMakeLists.txt new file mode 100644 index 000000000000..d2487d05bf2c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/adc/adc.cpp new file mode 100644 index 000000000000..aae84e2b8812 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/adc/adc.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include + +#include + +//todo S32K add ADC fior now steal the kinetis one +#include +#include + + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* ADC register accessors */ + +#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg)) + +#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */ +#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */ +#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */ +#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */ +#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */ +#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */ +#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */ +#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */ +#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */ +#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */ +#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */ +#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */ +#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */ +#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ + +int px4_arch_adc_init(uint32_t base_address) +{ + /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + + irqstate_t flags = px4_enter_critical_section(); + + _REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1; + rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8; + rCFG2(1) = 0; + rSC2(1) = ADC_SC2_REFSEL_DEFAULT; + + px4_leave_critical_section(flags); + + /* Clear the CALF and begin the calibration */ + + rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF; + + while ((rSC1A(1) & ADC_SC1_COCO) == 0) { + usleep(100); + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + } + + /* dummy read to clear COCO of calibration */ + + int32_t r = rRA(1); + + /* Check the state of CALF at the end of calibration */ + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + + /* Calculate the calibration values for single ended positive */ + + r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ; + r = 0x8000U | (r >> 1U); + rPG(1) = r; + + /* Calculate the calibration values for double ended Negitive */ + + r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ; + r = 0x8000U | (r >> 1U); + rMG(1) = r; + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + + rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* 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 -1; + } + } + + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + irqstate_t flags = px4_enter_critical_section(); + _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; + px4_leave_critical_section(flags); +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* clear any previous COCC */ + rRA(1); + + /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ + rSC1A(1) = ADC_SC1_ADCH(channel); + + /* wait for the conversion to complete */ + const hrt_abstime now = hrt_absolute_time(); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 10) { + px4_leave_critical_section(flags); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint32_t result = rRA(1); + + px4_leave_critical_section(flags); + + return result; +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); +} + +uint32_t px4_arch_adc_dn_fullcount() +{ + return 1 << 12; // 12 bit ADC +} diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/board_critmon/CMakeLists.txt new file mode 100644 index 000000000000..26cdb4bbc2c0 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/nxp/s32k1xx/board_critmon/board_critmon.c new file mode 100644 index 000000000000..14e7a3bb3bb4 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt new file mode 100644 index 000000000000..e818a473362e --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/board_reset/board_reset.c b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.c new file mode 100644 index 000000000000..bc552174a39a --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_reset.c + * Implementation of kinetis based Board RESET API + */ + +#include +#include +#include + + +#ifdef CONFIG_BOARDCTL_RESET + +/**************************************************************************** + * Public functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_reset + * + * Description: + * Reset board. Support for this function is required by board-level + * logic if CONFIG_BOARDCTL_RESET is selected. + * + * Input Parameters: + * status - Status information provided with the reset event. This + * meaning of this status information is board-specific. If not + * used by a board, the value zero may be provided in calls to + * board_reset(). + * + * Returned Value: + * If this function returns, then it was not possible to power-off the + * board due to some constraints. The return value int this case is a + * board-specific reason for the failure to shutdown. + * + ****************************************************************************/ + +int board_reset(int status) +{ + up_systemreset(); + return 0; +} + +#endif /* CONFIG_BOARDCTL_RESET */ + + +int board_set_bootload_mode(board_reset_e mode) +{ + uint32_t regvalue = 0; + + switch (mode) { + case board_reset_normal: + case board_reset_extended: + break; + + case board_reset_enter_bootloader: + regvalue = 0xb007b007; + break; + + default: + return -EINVAL; + } + +//todo need nvram for reboot to botloader + UNUSED(regvalue); + //*((uint32_t *) KINETIS_VBATR_BASE) = regvalue; + return OK; +} + + +void board_system_reset(int status) +{ + board_reset(status); + + while (1); +} diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/CMakeLists.txt new file mode 100644 index 000000000000..3edd775b6a63 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c new file mode 100644 index 000000000000..ae0c898561cb --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c @@ -0,0 +1,936 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2017 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 Kinetis TPM 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/s32k1xx_sim.h" + +//todo:stubs +#define HRT_COUNTER_PERIOD 65536 +#define HRT_COUNTER_SCALE(_c) (_c) + +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 = 0; //rCNT todo:fix this; + + /* + * 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; +} +#if 0 + +#include "kinetis_tpm.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_TPM_FREQ /* The input clock frequency to the TPM block */ +#define HRT_TIMER_BASE CAT(CAT(KINETIS_TPM, HRT_TIMER),_BASE) /* The Base address of the TPM */ +#define HRT_TIMER_VECTOR CAT(KINETIS_IRQ_TPM, HRT_TIMER) /* The TPM Interrupt vector */ +#define HRT_SIM_SCGC2_TPM CAT(SIM_SCGC2_TPM, HRT_TIMER) /* The Clock Gating enable bit for this TPM */ + +#if HRT_TIMER == 1 && defined(CONFIG_KINETIS_TPM1) +# error must not set CONFIG_KINETIS_TPM1=y and HRT_TIMER=1 +#elif HRT_TIMER == 2 && defined(CONFIG_KINETIS_TPM2) +# error must not set CONFIG_STM32_TIM2=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 16-bit timer/counter clocked +* at 1MHz. The high-resolution timer need only guarantee that it +* not wrap more than once in the 50ms period for absolute time to +* be consistently maintained. +* +* The minimum deadline must be such that the time taken between +* reading a time and writing a deadline to the timer cannot +* result in missing the deadline. +*/ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* +* Period of the free-running counter, in microseconds. +*/ +#define HRT_COUNTER_PERIOD 65536 + +/* +* Scaling factor(s) for the free-running counter; convert an input +* in counts to a time in microseconds. +*/ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* Timer register accessors */ + +#define REG(_reg) _REG(HRT_TIMER_BASE + (_reg)) + +#define rSC REG(KINETIS_TPM_SC_OFFSET) +#define rCNT REG(KINETIS_TPM_CNT_OFFSET) +#define rMOD REG(KINETIS_TPM_MOD_OFFSET) +#define rC0SC REG(KINETIS_TPM_C0SC_OFFSET) +#define rC0V REG(KINETIS_TPM_C0V_OFFSET) +#define rC1SC REG(KINETIS_TPM_C1SC_OFFSET) +#define rC1V REG(KINETIS_TPM_C1V_OFFSET) +#define rSTATUS REG(KINETIS_TPM_STATUS_OFFSET) +#define rCOMBINE REG(KINETIS_TPM_COMBINE_OFFSET) +#define rPOL REG(KINETIS_TPM_POL_OFFSET) +#define rFILTER REG(KINETIS_TPM_FILTER_OFFSET) +#define rQDCTRL REG(KINETIS_TPM_QDCTRL_OFFSET) +#define rCONF REG(KINETIS_TPM_CONF_OFFSET) + +/* +* Specific registers and bits used by HRT sub-functions +*/ + +# define rCNV_HRT CAT3(rC, HRT_TIMER_CHANNEL, V) /* Channel Value Register used by HRT */ +# define rCNSC_HRT CAT3(rC, HRT_TIMER_CHANNEL, SC) /* Channel Status and Control Register used by HRT */ +# define STATUS_HRT CAT3(TPM_STATUS_CH, HRT_TIMER_CHANNEL, F) /* Capture and Compare Status Register used by HRT */ + +#if (HRT_TIMER_CHANNEL != 0) && (HRT_TIMER_CHANNEL != 1) +# error HRT_TIMER_CHANNEL must be a value between 0 and 1 +#endif + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* 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 provide null operations */ + +# define STATUS_PPM 0 +# define POL_PPM 0 +# define CNSC_PPM 0 + +#else + +/* Specific registers and bits used by PPM sub-functions */ + +#define rCNV_PPM CAT3(rC,HRT_PPM_CHANNEL,V) /* Channel Value Register used by PPM */ +#define rCNSC_PPM CAT3(rC,HRT_PPM_CHANNEL,SC) /* Channel Status and Control Register used by PPM */ +#define STATUS_PPM CAT3(TPM_STATUS_CH, HRT_PPM_CHANNEL ,F) /* Capture and Compare Status Register used by PPM */ +#define CNSC_PPM (TPM_CnSC_CHIE | TPM_CnSC_ELSB | TPM_CnSC_ELSA) /* Input Capture configuration both Edges, interrupt */ + +/* Sanity checking */ + +#if (HRT_PPM_CHANNEL != 0) && (HRT_PPM_CHANNEL != 1) +# error HRT_PPM_CHANNEL must be a value between 0 and 1 +#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 uint16_t ppm_edge_history[EDGE_BUFFER_COUNT]; +unsigned ppm_edge_next; + +/* PPM pulse history */ + +__EXPORT uint16_t ppm_pulse_history[EDGE_BUFFER_COUNT]; +unsigned ppm_pulse_next; +# endif + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/** PPM decoder state machine */ +struct { + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +static void hrt_ppm_decode(uint32_t status); +#endif /* HRT_PPM_CHANNEL */ + +/** + * Initialize the timer we are going to use. + */ +static void hrt_tim_init(void) +{ + + /* Select a the clock source to the TPM */ + + uint32_t regval = _REG(KINETIS_SIM_SOPT2); + regval &= ~(SIM_SOPT2_TPMSRC_MASK); + regval |= BOARD_TPM_CLKSRC; + _REG(KINETIS_SIM_SOPT2) = regval; + + + /* Enabled System Clock Gating Control for TPM */ + + regval = _REG(KINETIS_SIM_SCGC2); + regval |= HRT_SIM_SCGC2_TPM; + _REG(KINETIS_SIM_SCGC2) = regval; + + + /* claim our interrupt vector */ + + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL); + + /* disable and configure the timer */ + + rSC = TPM_SC_TOF; + + rCNT = 0; + rMOD = HRT_COUNTER_PERIOD - 1; + + rSTATUS = (TPM_STATUS_TOF | STATUS_HRT | STATUS_PPM); + rCNSC_HRT = (TPM_CnSC_CHF | TPM_CnSC_CHIE | TPM_CnSC_MSA); + rCNSC_PPM = (TPM_CnSC_CHF | CNSC_PPM); + rCOMBINE = 0; + rPOL = 0; + rFILTER = 0; + rQDCTRL = 0; + rCONF = TPM_CONF_DBGMODE_CONT; + + /* set an initial capture a little ways off */ + + rCNV_PPM = 0; + rCNV_HRT = 1000; + + /* enable the timer */ + + rSC |= (TPM_SC_TOIE | TPM_SC_CMOD_LPTPM_CLK | TPM_SC_PS_DIV16); + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +#ifdef HRT_PPM_CHANNEL +/** + * Handle the PPM decoder state machine. + */ +static void hrt_ppm_decode(uint32_t status) +{ + uint16_t count = rCNV_PPM; + uint16_t width; + uint16_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 = rSTATUS; + + /* ack the interrupts we just read */ + + rSTATUS = 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 *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrtinfo("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + hrtinfo("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + hrtinfo("due soon\n"); + deadline = next->deadline; + } + } + + hrtinfo("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + rCNV_HRT = latency_baseline = deadline & 0xffff; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +#endif /* HRT_TIMER */ +#endif diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/adc.h new file mode 100644 index 000000000000..6551882e349a --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/adc.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 + +#define SYSTEM_ADC_BASE 0 // not used on s32k1xx + +#include diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..5df17d2d3167 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_io_timer.h + * + * stm32-specific PWM output data. + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#ifdef BOARD_NUM_IO_TIMERS +#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS +#else +#define MAX_IO_TIMERS 2 +#endif +#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 capture use + *** Note that the clock_freq is set to the source in the clock tree that + *** feeds this specific timer. This can differs by Timer! + *** 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 channel GPIO for PWM */ + uint32_t gpio_in; /* The timer channel GPIO for Capture */ + uint8_t timer_index; /* 0 based index in the io_timers_t table */ + uint8_t timer_channel; /* 1 based channel index GPIO_FTMt_CHcIN = c+1) */ +} timer_io_channels_t; + + +typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt, + 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/s32k1xx/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/CMakeLists.txt new file mode 100644 index 000000000000..401bf06ef5dd --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c new file mode 100644 index 000000000000..a1e349077181 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c @@ -0,0 +1,455 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_input_capture.c + * + * Servo driver supporting input capture connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have input pins. + * + * Require an interrupt. + * + * The use of thie interface is mutually exclusive with the pwm + * because the same timers are used and there is a resource contention + * with the ARR as it sets the pwm rate and in this driver needs to match + * that of the hrt to back calculate the actual point in time the edge + * was detected. + * + * This is accomplished by taking the difference between the current + * count rCNT snapped at the time interrupt and the rCCRx captured on the + * edge transition. This delta is applied to hrt time and the resulting + * value is the absolute system time the edge occured. + * + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include "hardware/kinetis_sim.h" +#include "hardware/kinetis_ftm.h" + + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + + +/* Timer register accessors */ + +#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET) + +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 = _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 (filter > FTM_FILTER_CH0FVAL_MASK) { + return -EINVAL; + } + + if (edge > Both) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + /* This register selects the filter value for the inputs of channels. + Channels 4, 5, 6 and 7 do not have an input filter. + */ + if (filter && timer_io_channels[channel].timer_channel - 1 > 3) { + return -EINVAL; + } + + + if (edge == Disabled) { + + io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); + input_capture_unbind(channel); + + } else { + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + input_capture_bind(channel, callback, context); + + rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); + + if (rv != 0) { + return rv; + } + + rv = up_input_capture_set_filter(channel, filter); + + if (rv == 0) { + rv = up_input_capture_set_trigger(channel, edge); + + if (rv == 0) { + rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel); + } + } + } + } + + return rv; +} + +int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -EINVAL; + + if (timer_io_channels[channel].timer_channel - 1 <= 3) { + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + rv = OK; + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rFILTER(timer) & FTM_FILTER_CH0FVAL_MASK; + *filter = (rvalue >> FTM_FILTER_CH0FVAL_SHIFT); + break; + + case 2: + rvalue = rFILTER(timer) & FTM_FILTER_CH1FVAL_MASK; + *filter = (rvalue >> FTM_FILTER_CH1FVAL_SHIFT); + break; + + case 3: + rvalue = rFILTER(timer) & FTM_FILTER_CH2FVAL_MASK; + *filter = (rvalue >> FTM_FILTER_CH2FVAL_SHIFT); + break; + + case 4: + rvalue = rFILTER(timer) & FTM_FILTER_CH3FVAL_MASK; + *filter = (rvalue >> FTM_FILTER_CH3FVAL_SHIFT); + break; + + default: + rv = -EIO; + } + } + } + } + + return rv; +} +int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) +{ + if (filter > FTM_FILTER_CH0FVAL_MASK) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue; + + irqstate_t flags = px4_enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + case 1: + rvalue = rFILTER(timer) & ~FTM_FILTER_CH0FVAL_MASK; + rvalue |= (filter << FTM_FILTER_CH0FVAL_SHIFT); + rFILTER(timer) = rvalue; + break; + + case 2: + rvalue = rFILTER(timer) & ~FTM_FILTER_CH1FVAL_MASK; + rvalue |= (filter << FTM_FILTER_CH1FVAL_SHIFT); + rFILTER(timer) = rvalue; + break; + + case 3: + rvalue = rFILTER(timer) & ~FTM_FILTER_CH2FVAL_MASK; + rvalue |= (filter << FTM_FILTER_CH2FVAL_SHIFT); + rFILTER(timer) = rvalue; + break; + + case 4: + rvalue = rFILTER(timer) & ~FTM_FILTER_CH2FVAL_MASK; + rvalue |= (filter << FTM_FILTER_CH2FVAL_SHIFT); + rFILTER(timer) = rvalue; + break; + + default: + rv = -EIO; + } + + px4_leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + + uint32_t timer = timer_io_channels[channel].timer_index; + uint16_t rvalue = _REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1)); + rvalue &= (FTM_CSC_MSB | FTM_CSC_MSA); + + switch (rvalue) { + + case (FTM_CSC_MSA): + *edge = Rising; + break; + + case (FTM_CSC_MSB): + *edge = Falling; + break; + + case (FTM_CSC_MSB|FTM_CSC_MSA): + *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 = FTM_CSC_MSA; + break; + + case Falling: + edge_bits = FTM_CSC_MSB; + break; + + case Both: + edge_bits = (FTM_CSC_MSB | FTM_CSC_MSA); + break; + + default: + return -EINVAL;; + } + + uint32_t timer = timer_io_channels[channel].timer_index; + irqstate_t flags = px4_enter_critical_section(); + uint32_t rvalue = _REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1)); + rvalue &= (FTM_CSC_MSB | FTM_CSC_MSA); + rvalue |= edge_bits; + _REG32(timer, KINETIS_FTM_CSC_OFFSET(timer_io_channels[channel].timer_channel - 1)) = 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/s32k1xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c new file mode 100644 index 000000000000..b8ffe9068410 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c @@ -0,0 +1,891 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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 Kinetis FTM timer blocks. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include "hardware/kinetis_sim.h" +#include "hardware/kinetis_ftm.h" + +/* The FTM pre-scalers are limited to Divide by 2^n where n={1-7} + * Therefore we use Y1 at 16 Mhz to drive FTM_CLKIN0 (PCT12) + * and use this at a 16Mhz clock for FTM0, FTM2 and FTM3. + * + * FTM0 will drive FMU_CH1-4, FTM3 will drive FMU_CH5,6, and + * U_TRI. FTM2 will be used as input capture on U_ECH + */ +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 1000000 +#endif + +#if !defined(BOARD_ONESHOT_FREQ) +#define BOARD_ONESHOT_FREQ 8000000 +#endif + +#define FTM_SRC_CLOCK_FREQ 16000000 + +#define MAX_CHANNELS_PER_TIMER 8 + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_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_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) + +#define CnSC_PWMIN_INIT 0 // TBD + +// 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) +{ + /* Read the count at the time of the interrupt */ + + uint16_t count = rCNT(timer_index); + + /* Read the HRT at the time of the interrupt */ + + hrt_abstime now = hrt_absolute_time(); + + const io_timers_t *tmr = &io_timers[timer_index]; + + /* What is pending */ + + uint32_t statusr = rSTATUS(timer_index); + + /* Acknowledge all that are pending */ + + rSTATUS(timer_index) = 0; + + /* Iterate over the timer_io_channels table */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + + uint16_t chan = 1 << chan_index; + + if (statusr & chan) { + + io_timer_channel_stats[chan_index].isr_cout++; + + /* Call the client to read the rCnV etc and clear the CHnF */ + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, + chan_index, &timer_io_channels[chan_index], + now, count, _REG32(tmr->base, KINETIS_FTM_CV_OFFSET(chan_index))); + } + } + + /* Did it set again during call out ?*/ + + if (rSTATUS(timer_index) & chan) { + + /* Error we has a second edge before we serviced the fist */ + + io_timer_channel_stats[chan_index].overflows++; + } + } + + 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 validate_timer_index(unsigned timer) +{ + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +static inline int is_timer_uninitalized(unsigned timer) +{ + int rv = 0; + + if (once & 1 << timer) { + rv = -EBUSY; + } + + return rv; +} + +static inline void set_timer_initalized(unsigned timer) +{ + once |= 1 << timer; +} + +static inline void set_timer_deinitalized(unsigned timer) +{ + once &= ~(1 << timer); +} + +static inline int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static inline int get_timers_firstchannels(unsigned timer) +{ + int channel = -1; + + if (validate_timer_index(timer) == 0) { + channel = timer_io_channels[io_timers[timer].first_channel_index].timer_channel; + } + + return channel; +} + +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + const io_timers_t *tmr = &io_timers[timer]; + + /* Gather the channel bits that belong to the timer */ + + for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + +static inline int is_channels_timer_uninitalized(unsigned channel) +{ + return is_timer_uninitalized(channels_timer(channel)); +} + +int io_timer_is_channel_free(unsigned channel) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { + rv = -EBUSY; + } + } + + return rv; +} + +int io_timer_validate_channel_index(unsigned channel) +{ + int rv = -EINVAL; + + if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { + + unsigned timer = timer_io_channels[channel].timer_index; + + /* test timer for validity */ + + if ((io_timers[timer].base != 0) && + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { + rv = 0; + } + } + + return rv; +} + +int io_timer_get_mode_channels(io_timer_channel_mode_t mode) +{ + if (mode < IOTimerChanModeSize) { + return channel_allocations[mode]; + } + + return 0; +} + +int io_timer_get_channel_mode(unsigned channel) +{ + io_timer_channel_allocation_t bit = 1 << channel; + + for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { + if (bit & channel_allocations[mode]) { + return mode; + } + } + + return -1; +} + +static 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 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) = (BOARD_PWM_FREQ / rate) - 1; + rSC(timer) = save; + + /* generate an update event; reloads the counter and all registers */ + rSYNC(timer) = FTM_SYNC; + + px4_leave_critical_section(flags); + + return 0; +} + +static inline uint32_t div2psc(int div) +{ + return 31 - __builtin_clz(div); +} + +static inline void io_timer_set_oneshot_mode(unsigned timer) +{ + /* Ideally, we would want per channel One pulse mode in HW + * Alas The FTM anly support onshot capture) + * todo:We can do this in an ISR later + * But since we do not have that + * We try to get the longest rate we can. + * On 16 bit timers this is 4.68 Ms. + */ + + irqstate_t flags = px4_enter_critical_section(); + rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK); + rMOD(timer) = 0xffff; + rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / BOARD_ONESHOT_FREQ)); + px4_leave_critical_section(flags); +} + +static inline void io_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 / BOARD_PWM_FREQ)); + px4_leave_critical_section(flags); +} + +void io_timer_trigger(void) +{ + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + uint32_t action_cache[MAX_IO_TIMERS] = {0}; + int actions = 0; + + /* Pre-calculate the list of timers to Trigger */ + + for (int timer = 0; timer < MAX_IO_TIMERS; timer++) { + if (validate_timer_index(timer) == 0) { + int channels = get_timer_channels(timer); + + if (oneshots & channels) { + action_cache[actions++] = io_timers[timer].base; + } + } + } + + /* Now do them all wit the shortest delay in between */ + + irqstate_t flags = px4_enter_critical_section(); + + for (actions = 0; actions < MAX_IO_TIMERS && action_cache[actions] != 0; actions++) { + _REG32(action_cache[actions], KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC; + } + + 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 */ + + uint32_t regval = _REG(io_timers[timer].clock_register); + regval |= io_timers[timer].clock_bit; + _REG(io_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 */ + + io_timer_set_PWM_mode(timer); + + /* + * Note we do the Standard PWM Out init here + * default to updating at 50Hz + */ + + timer_set_rate(timer, 50); + + /* + * Note that the timer is left disabled with IRQ subs installed + * and active but DEIR bits are not set. + */ + + irq_attach(io_timers[timer].vectorno, io_timers[timer].handler, NULL); + + up_enable_irq(io_timers[timer].vectorno); + + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_rate(unsigned timer, unsigned rate) +{ + int rv = EBUSY; + + /* Get the channel bits that belong to the timer */ + + uint32_t channels = get_timer_channels(timer); + + /* 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(timer); + } + + } 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(timer); + } + + timer_set_rate(timer, 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 = timer_io_channels[channel].gpio_in; + uint32_t clearbits = CnSC_RESET; + uint32_t setbits = CnSC_CAPTURE_INIT; + + /* figure out the GPIO config first */ + + switch (mode) { + + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + setbits = CnSC_PWMOUT_INIT; + gpio = 0; + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + break; + + case IOTimerChanMode_NotUsed: + setbits = 0; + break; + + default: + return -EINVAL; + } + + int rv = allocate_channel(channel, mode); + + /* Valid channel should now be reserved in new mode */ + + if (rv >= 0) { + + /* Blindly try to initialize the timer - it will only do it once */ + + io_timer_init_timer(channels_timer(channel)); + + irqstate_t flags = px4_enter_critical_section(); + + /* Set up IO */ + if (gpio) { + px4_arch_configgpio(gpio); + } + + + unsigned timer = channels_timer(channel); + + /* configure the channel */ + + uint32_t chan = timer_io_channels[channel].timer_channel - 1; + + uint16_t rvalue = REG(timer, KINETIS_FTM_CSC_OFFSET(chan)); + rvalue &= ~clearbits; + rvalue |= setbits; + REG(timer, KINETIS_FTM_CSC_OFFSET(chan)) = rvalue; + + //if (gpio){ TODO: NEEDED? + REG(timer, KINETIS_FTM_CV_OFFSET(0)) = 0; + + 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) +{ + typedef struct action_cache_rp_t { + uint32_t cnsc_offset; + uint32_t cnsc_value; + uint32_t gpio; + } action_cache_rp_t; + struct action_cache_t { + uint32_t base; + uint32_t index; + action_cache_rp_t cnsc[MAX_CHANNELS_PER_TIMER]; + } action_cache[MAX_IO_TIMERS]; + + memset(action_cache, 0, sizeof(action_cache)); + + uint32_t bits = state ? CnSC_PWMOUT_INIT : 0; + + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_OneShot: + case IOTimerChanMode_Trigger: + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + if (state) { + bits |= FTM_CSC_CHIE; + } + + break; + + default: + return -EINVAL; + } + + /* Was the request for all channels in this mode ?*/ + + if (masks == IO_TIMER_ALL_MODES_CHANNELS) { + + /* Yes - we provide them */ + + masks = channel_allocations[mode]; + + } else { + + /* No - caller provided mask */ + + /* Only allow the channels in that mode to be affected */ + + masks &= channel_allocations[mode]; + + } + + /* Pre calculate all the changes */ + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + + if (io_timer_validate_channel_index(chan_index) == 0) { + uint32_t chan = timer_io_channels[chan_index].timer_channel - 1; + uint32_t timer = channels_timer(chan_index); + action_cache[timer].base = io_timers[timer].base; + action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + KINETIS_FTM_CSC_OFFSET(chan); + action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits; + + if ((state && + (mode == IOTimerChanMode_PWMOut || + mode == IOTimerChanMode_OneShot || + mode == IOTimerChanMode_Trigger))) { + action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out; + } + + action_cache[timer].index++; + } + } + } + + irqstate_t flags = px4_enter_critical_section(); + + + for (unsigned actions = 0; actions < arraySize(action_cache); actions++) { + uint32_t any_on = 0; + + if (action_cache[actions].base != 0) { + for (unsigned int index = 0; index < action_cache[actions].index; index++) { + + if (action_cache[actions].cnsc[index].gpio) { + px4_arch_configgpio(action_cache[actions].cnsc[index].gpio); + } + + _REG(action_cache[actions].cnsc[index].cnsc_offset) = action_cache[actions].cnsc[index].cnsc_value; + any_on |= action_cache[actions].cnsc[index].cnsc_value; + } + + /* Any On ?*/ + + /* Assume not */ + uint32_t regval = _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET); + regval &= ~(FTM_SC_CLKS_MASK); + + if (any_on != 0) { + + /* force an update to preload all registers */ + _REG32(action_cache[actions].base, KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC; + + /* arm requires the timer be enabled */ + regval |= (FTM_SC_CLKS_EXTCLK); + } + + _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET) = regval; + } + } + + 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 { + + /* configure the channel */ + + /* todo:This is a HACK! + * Was getting: + * servo 0 readback error, got 1001 expected 1000 + * None of the SW syncs seamed update the CV + * So we stop the counter to get an immediate update. + */ + uint32_t timer = channels_timer(channel); + irqstate_t flags = px4_enter_critical_section(); + uint32_t save = rSC(timer); + rSC(timer) = save & ~(FTM_SC_CLKS_MASK); + REG(timer, KINETIS_FTM_CV_OFFSET(timer_io_channels[channel].timer_channel - 1)) = value; + rSC(timer) = save; + 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), KINETIS_FTM_CV_OFFSET(timer_io_channels[channel].timer_channel - 1)); + } + } + + return value; +} + +uint32_t io_timer_get_group(unsigned timer) +{ + return get_timer_channels(timer); + +} diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c new file mode 100644 index 000000000000..9534f51df33f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_free_channel(channel); + current &= ~(1 << channel); + } + } + + // Now allocate the new set + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not PWM mode before + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + return OK; +} + +void up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) +{ + if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 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(group, rate); +} + +void up_pwm_update(void) +{ + io_timer_trigger(); +} + +int up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { + up_pwm_servo_set_rate_group_update(i, rate); + } + + return 0; +} + +uint32_t up_pwm_servo_get_rate_group(unsigned group) +{ + return io_timer_get_group(group); +} + +void +up_pwm_servo_arm(bool armed) +{ + io_timer_set_enable(armed, IOTimerChanMode_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/s32k1xx/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c new file mode 100644 index 000000000000..f2f002aa7527 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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/s32k1xx/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt new file mode 100644 index 000000000000..1b1504352612 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp new file mode 100644 index 000000000000..e59021d55677 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp @@ -0,0 +1,345 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file drv_led_pwm.cpp +* +* +*/ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include "hardware/kinetis_sim.h" +#include "hardware/kinetis_ftm.h" + +#if 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/s32k1xx/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/CMakeLists.txt new file mode 100644 index 000000000000..a2d2f8144bcc --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp new file mode 100644 index 000000000000..df36cd725c79 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (C) 2017-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 "hardware/kinetis_sim.h" +#include "kinetis_tpm.h" + +#include +#include +#include +#include + +#include + +#define CAT3_(A, B, C) A##B##C +#define CAT3(A, B, C) CAT3_(A, B, C) + +/* 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 + +#ifndef TONE_ALARM_CLOCK +#define TONE_ALARM_CLOCK (BOARD_TPM_FREQ/(1 << (TONE_ALARM_TIMER_PRESCALE >> TPM_SC_PS_SHIFT))) +#endif + +/* Period of the free-running counter, in microseconds. */ +#ifndef TONE_ALARM_COUNTER_PERIOD +#define TONE_ALARM_COUNTER_PERIOD 65536 +#endif + +/* Tone Alarm configuration */ +#define TONE_ALARM_SIM_SCGC2_TPM CAT(SIM_SCGC2_TPM, TONE_ALARM_TIMER) /* The Clock Gating enable bit for this TPM */ +#define TONE_ALARM_TIMER_BASE CAT(CAT(KINETIS_TPM, TONE_ALARM_TIMER),_BASE) /* The Base address of the TPM */ +#define TONE_ALARM_TIMER_CLOCK BOARD_TPM_FREQ /* The input clock frequency to the TPM block */ +#define TONE_ALARM_TIMER_PRESCALE TPM_SC_PS_DIV16 /* The constant Prescaler */ +#define TONE_ALARM_TIMER_VECTOR CAT(KINETIS_IRQ_TPM, TONE_ALARM_TIMER) /* The TPM Interrupt vector */ + +#if TONE_ALARM_TIMER == 1 && defined(CONFIG_KINETIS_TPM1) +# error must not set CONFIG_KINETIS_TPM1=y and TONE_ALARM_TIMER=1 +#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_KINETIS_TPM2) +# error must not set CONFIG_STM32_TIM2=y and TONE_ALARM_TIMER=2 +#endif + +/* Tone Alarm clock must be a multiple of 1MHz greater than 1MHz. */ +#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_CLOCK) != 0 +# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_CLOCK +# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz +#endif + +#if (TONE_ALARM_CHANNEL != 0) && (TONE_ALARM_CHANNEL != 1) +# error TONE_ALARM_CHANNEL must be a value between 0 and 1 +#endif + + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* Timer register accessors */ +#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg)) + +#define rC0SC REG(KINETIS_TPM_C0SC_OFFSET) +#define rC0V REG(KINETIS_TPM_C0V_OFFSET) +#define rC1SC REG(KINETIS_TPM_C1SC_OFFSET) +#define rC1V REG(KINETIS_TPM_C1V_OFFSET) +#define rCNT REG(KINETIS_TPM_CNT_OFFSET) +#define rCOMBINE REG(KINETIS_TPM_COMBINE_OFFSET) +#define rCONF REG(KINETIS_TPM_CONF_OFFSET) +#define rFILTER REG(KINETIS_TPM_FILTER_OFFSET) +#define rMOD REG(KINETIS_TPM_MOD_OFFSET) +#define rPOL REG(KINETIS_TPM_POL_OFFSET) +#define rQDCTRL REG(KINETIS_TPM_QDCTRL_OFFSET) +#define rSC REG(KINETIS_TPM_SC_OFFSET) +#define rSTATUS REG(KINETIS_TPM_STATUS_OFFSET) + +/* Specific registers and bits used by Tone Alarm sub-functions. */ +# define rCNV CAT3(rC, TONE_ALARM_CHANNEL, V) /* Channel Value Register used by Tone alarm */ +# define rCNSC CAT3(rC, TONE_ALARM_CHANNEL, SC) /* Channel Status and Control Register used by Tone alarm */ +# define STATUS CAT3(TPM_STATUS_CH, TONE_ALARM_CHANNEL, F) /* Capture and Compare Status Register used by Tone alarm */ + +namespace ToneAlarmInterface +{ +void init() +{ +#ifdef GPIO_TONE_ALARM_NEG + px4_arch_configgpio(GPIO_TONE_ALARM_NEG); +#else + // Configure the GPIO to the idle state. + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +#endif + + // Select a the clock source to the TPM. + uint32_t regval = _REG(KINETIS_SIM_SOPT2); + + regval &= ~(SIM_SOPT2_TPMSRC_MASK); + regval |= BOARD_TPM_CLKSRC; + + _REG(KINETIS_SIM_SOPT2) = regval; + + // Enabled System Clock Gating Control for TPM. + regval = _REG(KINETIS_SIM_SCGC2); + regval |= TONE_ALARM_SIM_SCGC2_TPM; + _REG(KINETIS_SIM_SCGC2) = regval; + + // Disable and configure the timer. + rSC = TPM_SC_TOF; + rCNT = 0; + rMOD = TONE_ALARM_COUNTER_PERIOD - 1; + rSTATUS = (TPM_STATUS_TOF | STATUS); + + // Configure for output compare to toggle on over flow. + rCNSC = (TPM_CnSC_CHF | TPM_CnSC_MSA | TPM_CnSC_ELSA); + + rCOMBINE = 0; + rPOL = 0; + rFILTER = 0; + rQDCTRL = 0; + rCONF = TPM_CONF_DBGMODE_CONT; + + rCNV = 0; // Toggle the CC output each time the count passes 0. + rSC |= (TPM_SC_CMOD_LPTPM_CLK | TONE_ALARM_TIMER_PRESCALE); // Enable the timer. + rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this. +} + +void start_note(unsigned frequency) +{ + // Calculate the signal switching period. + // (Signal switching period is one half of the frequency period). + float signal_period = (1.0f / frequency) * 0.5f; + + // Calculate the hardware clock divisor rounded to the nearest integer. + unsigned int divisor = roundf(signal_period * TONE_ALARM_CLOCK); + + rCNT = 0; + rMOD = divisor; // Load new signal switching period. + rSC |= (TPM_SC_CMOD_LPTPM_CLK); + + // Configure the GPIO to enable timer output. + px4_arch_configgpio(GPIO_TONE_ALARM); +} + +void stop_note() +{ + // Stop the current note. + rSC &= ~TPM_SC_CMOD_MASK; + + // Ensure the GPIO is not driving the speaker. + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +} + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/version/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/version/CMakeLists.txt new file mode 100644 index 000000000000..3303bdea4372 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/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/s32k1xx/version/board_identity.c b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c new file mode 100644 index 000000000000..df6c5a4a0c6e --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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_identity.c + * Implementation of Kientis based Board identity API + */ + +#include +#include +#include +#include +#include +#include + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID; + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +void board_get_uuid(uuid_byte_t uuid_bytes) +{ + uint32_t *chip_uuid = (uint32_t *) S32K1XX_SIM_UIDH; + uint8_t *uuid_words = uuid_bytes; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uint32_t current_uuid_bytes = SWAP_UINT32(chip_uuid[i]); + memcpy(uuid_words, ¤t_uuid_bytes, sizeof(uint32_t)); + uuid_words += sizeof(uint32_t); + } +} + +void board_get_uuid32(uuid_uint32_t uuid_words) +{ + board_get_uuid(*(uuid_byte_t *) uuid_words); +} + +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); + 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/s32k1xx/version/board_mcu_version.c b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_mcu_version.c new file mode 100644 index 000000000000..adbbb98d2113 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_mcu_version.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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_mcu_version.c + * Implementation of Kinetis based SoC version API + */ + +#include +#include + +#include "up_arch.h" +#include "hardware/s32k1xx_sim.h" + +//todo upstrem chip id defs +#define CHIP_TAG "S32K1??" +#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1 + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + uint32_t sim_sdid = getreg32(S32K1XX_SIM_SDID); + UNUSED(sim_sdid); + static char chip[sizeof(CHIP_TAG)] = CHIP_TAG; + + chip[CHIP_TAG_LEN - 2] = '0' /*+ ((sim_sdid & SIM_SDID_FAMILYID_MASK) >> SIM_SDID_FAMILYID_SHIFT) */; + chip[CHIP_TAG_LEN - 1] = '0' /*+ ((sim_sdid & SIM_SDID_SUBFAMID_MASK) >> SIM_SDID_SUBFAMID_SHIFT)*/; + *revstr = chip; + *rev = '0' /*+ ((sim_sdid & SIM_SDID_REVID_MASK) >> SIM_SDID_REVID_SHIFT)*/; + + if (errata) { + *errata = NULL; + } + + return 0; +} From 370bb39dafc6b787614defaacd8496ba9ada2cff Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 1 Feb 2020 15:02:48 -0800 Subject: [PATCH 3/4] rddrone-uavcan146:Board Added --- boards/nxp/rddrone-uavcan146/Kconfig | 4 + boards/nxp/rddrone-uavcan146/default.cmake | 19 ++ .../nxp/rddrone-uavcan146/firmware.prototype | 13 + .../rddrone-uavcan146/nuttx-config/Kconfig | 17 ++ .../nuttx-config/include/board.h | 153 ++++++++++++ .../nuttx-config/nsh/defconfig | 74 ++++++ .../nuttx-config/scripts/s32k146.cfg | 58 +++++ .../nuttx-config/scripts/script.ld | 152 ++++++++++++ .../nuttx-config/scripts/sram.ld | 129 ++++++++++ .../nxp/rddrone-uavcan146/src/CMakeLists.txt | 52 ++++ boards/nxp/rddrone-uavcan146/src/autoleds.c | 162 +++++++++++++ .../nxp/rddrone-uavcan146/src/board_config.h | 153 ++++++++++++ boards/nxp/rddrone-uavcan146/src/boot.c | 94 ++++++++ boards/nxp/rddrone-uavcan146/src/bringup.c | 146 +++++++++++ boards/nxp/rddrone-uavcan146/src/buttons.c | 160 +++++++++++++ .../nxp/rddrone-uavcan146/src/clockconfig.c | 226 ++++++++++++++++++ boards/nxp/rddrone-uavcan146/src/init.c | 94 ++++++++ .../nxp/rddrone-uavcan146/src/periphclocks.c | 186 ++++++++++++++ boards/nxp/rddrone-uavcan146/src/spi.c | 196 +++++++++++++++ boards/nxp/rddrone-uavcan146/src/userleds.c | 112 +++++++++ 20 files changed, 2200 insertions(+) create mode 100644 boards/nxp/rddrone-uavcan146/Kconfig create mode 100644 boards/nxp/rddrone-uavcan146/default.cmake create mode 100644 boards/nxp/rddrone-uavcan146/firmware.prototype create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/Kconfig create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/include/board.h create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/nsh/defconfig create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/scripts/s32k146.cfg create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/scripts/script.ld create mode 100644 boards/nxp/rddrone-uavcan146/nuttx-config/scripts/sram.ld create mode 100644 boards/nxp/rddrone-uavcan146/src/CMakeLists.txt create mode 100644 boards/nxp/rddrone-uavcan146/src/autoleds.c create mode 100644 boards/nxp/rddrone-uavcan146/src/board_config.h create mode 100644 boards/nxp/rddrone-uavcan146/src/boot.c create mode 100644 boards/nxp/rddrone-uavcan146/src/bringup.c create mode 100644 boards/nxp/rddrone-uavcan146/src/buttons.c create mode 100644 boards/nxp/rddrone-uavcan146/src/clockconfig.c create mode 100644 boards/nxp/rddrone-uavcan146/src/init.c create mode 100644 boards/nxp/rddrone-uavcan146/src/periphclocks.c create mode 100644 boards/nxp/rddrone-uavcan146/src/spi.c create mode 100644 boards/nxp/rddrone-uavcan146/src/userleds.c diff --git a/boards/nxp/rddrone-uavcan146/Kconfig b/boards/nxp/rddrone-uavcan146/Kconfig new file mode 100644 index 000000000000..f72f3c094ce4 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# diff --git a/boards/nxp/rddrone-uavcan146/default.cmake b/boards/nxp/rddrone-uavcan146/default.cmake new file mode 100644 index 000000000000..9d7034ea6f3e --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/default.cmake @@ -0,0 +1,19 @@ +px4_add_board( + PLATFORM nuttx + VENDOR nxp + MODEL rddrone-uavcan146 + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + + UAVCAN_INTERFACES 2 + + DRIVERS + + MODULES + + SYSTEMCMDS + i2cdetect + + EXAMPLES + ) diff --git a/boards/nxp/rddrone-uavcan146/firmware.prototype b/boards/nxp/rddrone-uavcan146/firmware.prototype new file mode 100644 index 000000000000..bb3c62891a23 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 10000, + "magic": "PX4FWv1", + "description": "Firmware for the rddrone-uavcan146 board", + "image": "", + "build_time": 0, + "summary": "RDDRONE-UAVCAN146", + "version": "0.1", + "image_size": 0, + "image_maxsize": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/Kconfig b/boards/nxp/rddrone-uavcan146/nuttx-config/Kconfig new file mode 100644 index 000000000000..57cceef9e042 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-6 to provide timing signals from selected drivers. diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/include/board.h b/boards/nxp/rddrone-uavcan146/nuttx-config/include/board.h new file mode 100644 index 000000000000..c0ab9d1d96a0 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/include/board.h @@ -0,0 +1,153 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/include/board.h + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H +#define __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +# include +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Clocking *****************************************************************/ + +/* The RDDRONE-UAVCAN146 is fitted with a 8MHz Crystal */ + +#define BOARD_XTAL_FREQUENCY 8000000 + +/* The S32K146 will run at 112MHz */ + +/* LED definitions **********************************************************/ + +/* The RDDRONE-UAVCAN146 has one RGB LED: + * + * RedLED PTD15 (FTM0 CH0) + * GreenLED PTD16 (FTM0 CH1) + * BlueLED PTD0 (FTM0 CH2) + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in + * any way. The following definitions are used to access individual RGB + * components. + * + * The RGB components could, alternatively be controlled through PWM using + * the common RGB LED driver. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED_R 0 +#define BOARD_LED_G 1 +#define BOARD_LED_B 2 +#define BOARD_NLEDS 3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED_R_BIT (1 << BOARD_LED_R) +#define BOARD_LED_G_BIT (1 << BOARD_LED_G) +#define BOARD_LED_B_BIT (1 << BOARD_LED_B) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LEDs on board + * the RDDRONE-UAVCAN146. The following definitions describe how NuttX + * controls the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ------------------- ---------------------------- ----------------- + */ + +#define LED_STARTED 1 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 2 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 0 /* Interrupts enabled OFF OFF ON */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON OFF */ +#define LED_INIRQ 0 /* In an interrupt (no change) */ +#define LED_SIGNAL 0 /* In a signal handler (no change) */ +#define LED_ASSERTION 0 /* An assertion failed (no change) */ +#define LED_PANIC 4 /* The system has crashed FLASH OFF OFF */ +#undef LED_IDLE /* RDDRONE-UAVCAN146 in sleep mode (Not used) */ + +/* Button definitions *******************************************************/ + +/* The RDDRONE-UAVCAN146 supports two buttons: + * + * SW2 PTC12 + * SW3 PTC13 + */ + +#define BUTTON_SW2 0 +#define BUTTON_SW3 1 +#define NUM_BUTTONS 2 + +#define BUTTON_SW2_BIT (1 << BUTTON_SW2) +#define BUTTON_SW3_BIT (1 << BUTTON_SW3) + +/* Alternate function pin selections ****************************************/ + +/* By default, the serial console will be provided on the OpenSDA VCOM port: + * + * OpenSDA UART TX PTC7 (LPUART1_TX) + * OpenSDA UART RX PTC6 (LPUART1_RX) + */ + +#define PIN_LPUART0_CTS PIN_LPUART0_CTS_2 /* PTC8 */ +#define PIN_LPUART0_RTS PIN_LPUART0_RTS_2 /* PTC9 */ +#define PIN_LPUART0_RX PIN_LPUART0_RX_1 /* PTB0 */ +#define PIN_LPUART0_TX PIN_LPUART0_TX_1 /* PTB1 */ + +#define PIN_LPUART1_RX PIN_LPUART1_RX_1 /* PTC6 */ +#define PIN_LPUART1_TX PIN_LPUART1_TX_1 /* PTC7 */ + +/* SPI selections ***********************************************************/ + +#define PIN_LPSPI0_SCK PIN_LPSPI0_SCK_2 /* PTB2 */ +#define PIN_LPSPI0_MISO PIN_LPSPI0_SIN_2 /* PTB3 */ +#define PIN_LPSPI0_MOSI PIN_LPSPI0_SOUT_3 /* PTB4 */ +#define PIN_LPSPI0_PCS PIN_LPSPI0_PCS0_2 /* PTB5 */ + +/* I2C selections ***********************************************************/ + +#define PIN_LPI2C0_SCL PIN_LPI2C0_SCL_2 /* PTA3 */ +#define PIN_LPI2C0_SDA PIN_LPI2C0_SDA_2 /* PTA2 */ + +#endif /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */ diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/nsh/defconfig b/boards/nxp/rddrone-uavcan146/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..880b378f1b91 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/nsh/defconfig @@ -0,0 +1,74 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_ARCH_FPU is not set +# CONFIG_NSH_ARGCAT is not set +# CONFIG_NSH_CMDOPT_HEXDUMP is not set +# CONFIG_NSH_CMDPARMS is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="s32k1xx" +CONFIG_ARCH_CHIP_S32K146=y +CONFIG_ARCH_CHIP_S32K14X=y +CONFIG_ARCH_CHIP_S32K1XX=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=3997 +CONFIG_BUILTIN=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_FS_PROCFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2CTOOL_DEFFREQ=100000 +CONFIG_I2CTOOL_MAXADDR=0x7f +CONFIG_I2CTOOL_MAXBUS=0 +CONFIG_I2CTOOL_MINADDR=0x00 +CONFIG_INTELHEX_BINARY=y +CONFIG_LPUART0_RXBUFSIZE=64 +CONFIG_LPUART0_TXBUFSIZE=64 +CONFIG_LPUART1_RXBUFSIZE=64 +CONFIG_LPUART1_SERIAL_CONSOLE=y +CONFIG_LPUART1_TXBUFSIZE=64 +CONFIG_MAX_TASKS=16 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_MOTOROLA_SREC=y +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=8 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_READLINE=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=4 +CONFIG_PREALLOC_WDOGS=16 +CONFIG_RAM_SIZE=126976 +CONFIG_RAM_START=0x1fff0000 +CONFIG_RAW_BINARY=y +CONFIG_RR_INTERVAL=200 +CONFIG_S32K1XX_LPI2C0=y +CONFIG_S32K1XX_LPSPI0=y +CONFIG_S32K1XX_LPUART0=y +CONFIG_S32K1XX_LPUART1=y +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SPITOOL_DEFFREQ=400000 +CONFIG_SPITOOL_MAXBUS=0 +CONFIG_SPITOOL_PROGNAME="spi" +CONFIG_START_DAY=18 +CONFIG_START_MONTH=8 +CONFIG_START_YEAR=2019 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_SYMTAB_ORDEREDBYNAME=y +CONFIG_SYSTEM_I2CTOOL=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_SPITOOL=y +CONFIG_USER_ENTRYPOINT="nsh_main" \ No newline at end of file diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/s32k146.cfg b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/s32k146.cfg new file mode 100644 index 000000000000..749facc47c85 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/s32k146.cfg @@ -0,0 +1,58 @@ +# +# NXP S32K146 - 1x ARM Cortex-M4 @ up to 180 MHz +# + +adapter_khz 4000 +transport select swd + +if { [info exists CHIPNAME] } { + set _CHIPNAME $CHIPNAME +} else { + set _CHIPNAME s32k146 +} + +# +# M4 JTAG mode TAP +# +if { [info exists M4_JTAG_TAPID] } { + set _M4_JTAG_TAPID $M4_JTAG_TAPID +} else { + set _M4_JTAG_TAPID 0x4ba00477 +} + +# +# M4 SWD mode TAP +# +if { [info exists M4_SWD_TAPID] } { + set _M4_SWD_TAPID $M4_SWD_TAPID +} else { + set _M4_SWD_TAPID 0x2ba01477 +} + +source [find target/swj-dp.tcl] + +if { [using_jtag] } { + set _M4_TAPID $_M4_JTAG_TAPID +} else { + set _M4_TAPID $_M4_SWD_TAPID +} + +swj_newdap $_CHIPNAME m4 -irlen 4 -ircapture 0x1 -irmask 0xf \ + -expected-id $_M4_TAPID + +target create $_CHIPNAME.m4 cortex_m -chain-position $_CHIPNAME.m4 + +# S32K146 has 64+60 KB contiguous SRAM +if { [info exists WORKAREASIZE] } { + set _WORKAREASIZE $WORKAREASIZE +} else { + set _WORKAREASIZE 0x1F000 +} +$_CHIPNAME.m4 configure -work-area-phys 0x1FFF0000 \ + -work-area-size $_WORKAREASIZE -work-area-backup 0 + +$_CHIPNAME.m4 configure -rtos nuttx + +if { ![using_hla] } { + cortex_m reset_config vectreset +} diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/script.ld b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..8d08125616f1 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/script.ld @@ -0,0 +1,152 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/scripts/flash.ld + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The S32K146 has 1Mb of FLASH beginning at address 0x0000:0000 and + * 124Kb of SRAM beginning at address 0x1fff:0000 (plus 4Kb of FlexRAM) + * + * The on-chip RAM is split in two regions: SRAM_L and SRAM_U. The RAM is + * implemented such that the SRAM_L and SRAM_U ranges form a contiguous + * block in the memory map + * + * SRAM_L 1fff0000 - 1fffffff 64Kb + * SRAM_U 20000000 - 2000efff 60Kb + */ + +MEMORY +{ + vflash (rx) : ORIGIN = 0x00000000, LENGTH = 1K + pflash (rx) : ORIGIN = 0x00000400, LENGTH = 16 + dflash (rx) : ORIGIN = 0x00000410, LENGTH = 1023K-16 + sram (rwx) : ORIGIN = 0x1fff0000, LENGTH = 124K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flashcfg) +ENTRY(_stext) + +SECTIONS +{ + .vectors : + { + _stext = ABSOLUTE(.); + *(.vectors) + } > vflash + + .flashcfg : + { + *(.flashcfg) + } > pflash + + .text : + { + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > dflash + + .init_section : + { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > dflash + + .ARM.extab : + { + *(.ARM.extab*) + } >dflash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } >dflash + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > dflash + + _eronly = LOADADDR(.data); + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > dflash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/sram.ld b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/sram.ld new file mode 100644 index 000000000000..ff18d0ef33f9 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/nuttx-config/scripts/sram.ld @@ -0,0 +1,129 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/scripts/sram.ld + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The S32K146 has 1Mb of FLASH beginning at address 0x0000:0000 and + * 124Kb of SRAM beginning at address 0x1fff:0000 (plus 4Kb of FlexRAM) + * + * The on-chip RAM is split in two regions: SRAM_L and SRAM_U. The RAM is + * implemented such that the SRAM_L and SRAM_U ranges form a contiguous + * block in the memory map + * + * SRAM_L 1fff0000 - 1fffffff 64Kb + * SRAM_U 20000000 - 2000efff 60Kb + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x00000000, LENGTH = 1M + sram (rwx) : ORIGIN = 0x1fff0000, LENGTH = 124K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +SECTIONS +{ + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > sram + + .init_section : + { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > sram + + .ARM.extab : + { + *(.ARM.extab*) + } >sram + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } >sram + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/nxp/rddrone-uavcan146/src/CMakeLists.txt b/boards/nxp/rddrone-uavcan146/src/CMakeLists.txt new file mode 100644 index 000000000000..497340528e44 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2016, 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(drivers_board + autoleds.c + boot.c + bringup.c + buttons.c + clockconfig.c + init.c + periphclocks.c + spi.c + userleds.c +) + +target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + drivers__led # drv_led_start + px4_layer + ) diff --git a/boards/nxp/rddrone-uavcan146/src/autoleds.c b/boards/nxp/rddrone-uavcan146/src/autoleds.c new file mode 100644 index 000000000000..1f063e77c61b --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/autoleds.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_autoleds.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The RDDRONE-UAVCAN146 has one RGB LED: + * + * RedLED PTD15 (FTM0 CH0) + * GreenLED PTD16 (FTM0 CH1) + * BlueLED PTD0 (FTM0 CH2) + * + * An output of '1' illuminates the LED. + * + * If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the Freedom K66F. The following definitions describe how NuttX controls + * the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ------------------- ----------------------- ----------------- + * LED_STARTED NuttX has been started OFF OFF OFF + * LED_HEAPALLOCATE Heap has been allocated OFF OFF ON + * LED_IRQSENABLED Interrupts enabled OFF OFF ON + * LED_STACKCREATED Idle stack created OFF ON OFF + * LED_INIRQ In an interrupt (no change) + * LED_SIGNAL In a signal handler (no change) + * LED_ASSERTION An assertion failed (no change) + * LED_PANIC The system has crashed FLASH OFF OFF + * LED_IDLE K66 is in sleep mode (Optional, not used) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "up_internal.h" + +#include "s32k1xx_pin.h" +#include "board_config.h" + +#include + +#ifdef CONFIG_ARCH_LEDS + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Summary of all possible settings */ + +#define LED_NOCHANGE 0 /* LED_IRQSENABLED, LED_INIRQ, LED_SIGNAL, LED_ASSERTION */ +#define LED_OFF_OFF_OFF 1 /* LED_STARTED */ +#define LED_OFF_OFF_ON 2 /* LED_HEAPALLOCATE */ +#define LED_OFF_ON_OFF 3 /* LED_STACKCREATED */ +#define LED_ON_OFF_OFF 4 /* LED_PANIC */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + /* Configure LED GPIOs for output */ + + s32k1xx_pinconfig(GPIO_LED_R); + s32k1xx_pinconfig(GPIO_LED_G); + s32k1xx_pinconfig(GPIO_LED_B); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (led != LED_NOCHANGE) { + bool redon = false; + bool greenon = false; + bool blueon = false; + + switch (led) { + default: + case LED_OFF_OFF_OFF: + break; + + case LED_OFF_OFF_ON: + blueon = true; + break; + + case LED_OFF_ON_OFF: + greenon = true; + break; + + case LED_ON_OFF_OFF: + redon = true; + break; + } + + s32k1xx_gpiowrite(GPIO_LED_R, redon); + s32k1xx_gpiowrite(GPIO_LED_G, greenon); + s32k1xx_gpiowrite(GPIO_LED_B, blueon); + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (led == LED_ON_OFF_OFF) { + s32k1xx_gpiowrite(GPIO_LED_R, true); + s32k1xx_gpiowrite(GPIO_LED_G, false); + s32k1xx_gpiowrite(GPIO_LED_B, false); + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/nxp/rddrone-uavcan146/src/board_config.h b/boards/nxp/rddrone-uavcan146/src/board_config.h new file mode 100644 index 000000000000..db48f2b8ea44 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/board_config.h @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * + * Copyright (C) 2019 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * NXP rddrone-uavcan146 internal definitions + */ + +#pragma once + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include "hardware/s32k1xx_pinmux.h" +#include "s32k1xx_periphclocks.h" +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* RDDRONE-UAVCAN146 GPIOs **************************************************/ + +/* LEDs. The RDDRONE-UAVCAN146 has one RGB LED: + * + * RedLED PTD15 (FTM0 CH0) + * GreenLED PTD16 (FTM0 CH1) + * BlueLED PTD0 (FTM0 CH2) + * + * An output of '1' illuminates the LED. + */ + +#define GPIO_LED_R (PIN_PTD15 | GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO) +#define GPIO_LED_G (PIN_PTD16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO) +#define GPIO_LED_B (PIN_PTD0 | GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO) + +/* Buttons. The RDDRONE-UAVCAN146 supports two buttons: + * + * SW2 PTC12 + * SW3 PTC13 + */ + +#define GPIO_SW2 (PIN_PTC12 | PIN_INT_BOTH) +#define GPIO_SW3 (PIN_PTC13 | PIN_INT_BOTH) + +/* SPI chip selects */ + +/* Count of peripheral clock user configurations */ + +#define NUM_OF_PERIPHERAL_CLOCKS_0 15 + +/* I2C busses */ + +#define PX4_I2C_BUS_ONBOARD PX4_BUS_NUMBER_TO_PX4(1) +#define PX4_I2C_BUS_EXPANSION PX4_BUS_NUMBER_TO_PX4(0) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifndef __ASSEMBLY__ + +/* User peripheral configuration structure 0 */ + +extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[]; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k1xx_bringup + * + * Description: + * Perform architecture-specific initialization + * + * CONFIG_BOARD_LATE_INITIALIZE=y : + * Called from board_late_initialize(). + * + * CONFIG_BOARD_LATE_INITIALIZE=y && CONFIG_LIB_BOARDCTL=y : + * Called from the NSH library + * + ****************************************************************************/ + +int s32k1xx_bringup(void); + +/**************************************************************************** + * Name: s32k1xx_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the RDDRONE-UAVCAN146 + * board. + * + ****************************************************************************/ + +#ifdef CONFIG_S32K1XX_LPSPI +void s32k1xx_spidev_initialize(void); +#endif + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/rddrone-uavcan146/src/boot.c b/boards/nxp/rddrone-uavcan146/src/boot.c new file mode 100644 index 000000000000..4e58ab733ed5 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/boot.c @@ -0,0 +1,94 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_boot.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#include + +#include "board_config.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k1xx_board_initialize + * + * Description: + * All S32K1XX architectures must provide the following entry point. This + * entry point is called early in the initialization -- after all memory + * has been configured and mapped but before any devices have been + * initialized. + * + ****************************************************************************/ + +void s32k1xx_board_initialize(void) +{ +#ifdef CONFIG_ARCH_LEDS + /* Configure on-board LEDs if LED support has been selected. */ + + board_autoled_initialize(); +#endif +} + +/**************************************************************************** + * Name: board_late_initialize + * + * Description: + * If CONFIG_BOARD_LATE_INITIALIZE is selected, then an additional + * initialization call will be performed in the boot-up sequence to a + * function called board_late_initialize(). board_late_initialize() will + * be called immediately after up_initialize() is called and just before + * the initial application is started. This additional initialization + * phase may be used, for example, to initialize board-specific device + * drivers. + * + ****************************************************************************/ + +#ifdef CONFIG_BOARD_LATE_INITIALIZE +void board_late_initialize(void) +{ + /* Perform board-specific initialization */ + + s32k1xx_bringup(); +} +#endif diff --git a/boards/nxp/rddrone-uavcan146/src/bringup.c b/boards/nxp/rddrone-uavcan146/src/bringup.c new file mode 100644 index 000000000000..fbd6f581c4e9 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/bringup.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_bringup.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#ifdef CONFIG_BUTTONS +# include +#endif + +#ifdef CONFIG_USERLED +# include +#endif + +#ifdef CONFIG_I2C_DRIVER +# include "s32k1xx_pin.h" +# include +# include "s32k1xx_lpi2c.h" +#endif + +#include "board_config.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k1xx_bringup + * + * Description: + * Perform architecture-specific initialization + * + * CONFIG_BOARD_LATE_INITIALIZE=y : + * Called from board_late_initialize(). + * + * CONFIG_BOARD_LATE_INITIALIZE=n && CONFIG_LIB_BOARDCTL=y : + * Called from the NSH library + * + ****************************************************************************/ + +int s32k1xx_bringup(void) +{ + int ret = OK; + +#ifdef CONFIG_BUTTONS + /* Register the BUTTON driver */ + + ret = btn_lower_initialize("/dev/buttons"); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: btn_lower_initialize() failed: %d\n", ret); + } + +#endif + +#ifdef CONFIG_USERLED + /* Register the LED driver */ + + ret = userled_lower_initialize("/dev/userleds"); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: userled_lower_initialize() failed: %d\n", ret); + } + +#endif + +#ifdef CONFIG_FS_PROCFS + /* Mount the procfs file system */ + + ret = mount(NULL, "/proc", "procfs", 0, NULL); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret); + } + +#endif + +#ifdef CONFIG_S32K1XX_LPSPI + /* Configure SPI chip selects if 1) SPI is not disabled, and 2) the weak + * function s32k1xx_spidev_initialize() has been brought into the link. + */ + + s32k1xx_spidev_initialize(); +#endif + +#if defined(CONFIG_S32K1XX_LPI2C0) +#if defined(CONFIG_I2C_DRIVER) + FAR struct i2c_master_s *i2c; + i2c = s32k1xx_i2cbus_initialize(0); + + if (i2c == NULL) { + serr("ERROR: Failed to get I2C%d interface\n", bus); + + } else { + ret = i2c_register(i2c, 0); + + if (ret < 0) { + serr("ERROR: Failed to register I2C%d driver: %d\n", bus, ret); + s32k1xx_i2cbus_uninitialize(i2c); + } + } + +#endif +#endif + + return ret; +} diff --git a/boards/nxp/rddrone-uavcan146/src/buttons.c b/boards/nxp/rddrone-uavcan146/src/buttons.c new file mode 100644 index 000000000000..a6dc2a4cb17f --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/buttons.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_buttons.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The RDDRONE-UAVCAN146 supports two buttons: + * + * SW2 PTC12 + * SW3 PTC13 + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "s32k1xx_pin.h" +#include "board_config.h" + +#include + +#ifdef CONFIG_ARCH_BUTTONS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_button_initialize + * + * Description: + * board_button_initialize() must be called to initialize button resources. + * After that, board_buttons() may be called to collect the current state + * of all buttons or board_button_irq() may be called to register button + * interrupt handlers. + * + ****************************************************************************/ + +void board_button_initialize(void) +{ + /* Configure the GPIO pins as interrupting inputs. */ + + s32k1xx_pinconfig(GPIO_SW2); + s32k1xx_pinconfig(GPIO_SW3); +} + +/**************************************************************************** + * Name: board_buttons + ****************************************************************************/ + +uint32_t board_buttons(void) +{ + uint32_t ret = 0; + + if (s32k1xx_gpioread(GPIO_SW2)) { + ret |= BUTTON_SW2_BIT; + } + + if (s32k1xx_gpioread(GPIO_SW3)) { + ret |= BUTTON_SW3_BIT; + } + + return ret; +} + +/**************************************************************************** + * Button support. + * + * Description: + * board_button_initialize() must be called to initialize button resources. + * After that, board_buttons() may be called to collect the current state + * of all buttons or board_button_irq() may be called to register button + * interrupt handlers. + * + * After board_button_initialize() has been called, board_buttons() may be + * called to collect the state of all buttons. board_buttons() returns a + * 32-bit bit set with each bit associated with a button. See the + * BUTTON_*_BIT definitions in board.h for the meaning of each bit. + * + * board_button_irq() may be called to register an interrupt handler that + * will be called when a button is depressed or released. The ID value is + * a button enumeration value that uniquely identifies a button resource. + * See the BUTTON_* definitions in board.h for the meaning of enumeration + * value. + * + ****************************************************************************/ + +#ifdef CONFIG_ARCH_IRQBUTTONS +int board_button_irq(int id, xcpt_t irqhandler, FAR void *arg) +{ + uint32_t pinset; + int ret; + + /* Map the button id to the GPIO bit set. */ + + if (id == BUTTON_SW2) { + pinset = GPIO_SW2; + + } else if (id == BUTTON_SW3) { + pinset = GPIO_SW3; + + } else { + return -EINVAL; + } + + /* The button has already been configured as an interrupting input (by + * board_button_initialize() above). + * + * Attach the new button handler. + */ + + ret = s32k1xx_pinirqattach(pinset, irqhandler, NULL); + + if (ret >= 0) { + /* Then make sure that interrupts are enabled on the pin */ + + s32k1xx_pinirqenable(pinset); + } + + return ret; +} +#endif +#endif /* CONFIG_ARCH_BUTTONS */ diff --git a/boards/nxp/rddrone-uavcan146/src/clockconfig.c b/boards/nxp/rddrone-uavcan146/src/clockconfig.c new file mode 100644 index 000000000000..00bcb5d18b5f --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/clockconfig.c @@ -0,0 +1,226 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_clockconfig.c + * + * Copyright (C) 2019 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. + * + * Most of the settings within this file derives from NXP sample code for + * the S32K1XX MCUs. That sample code has this licensing information: + * + * Copyright (c) 2013 - 2015, Freescale Semiconductor, Inc. + * Copyright 2016-2018 NXP + * All rights reserved. + * + * THIS SOFTWARE IS PROVIDED BY NXP "AS IS" AND ANY EXPRESSED 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 NXP OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "s32k1xx_clockconfig.h" +#include "s32k1xx_start.h" +#include "board_config.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Each S32K1XX board must provide the following initialized structure. + * This is needed to establish the initial board clocking. + */ + +const struct clock_configuration_s g_initial_clkconfig = { + .scg = + { + .sirc = + { + .range = SCG_SIRC_RANGE_HIGH, /* RANGE - High range (8 MHz) */ + .div1 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SIRCDIV1 */ + .div2 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SIRCDIV2 */ + .initialize = true, /* Initialize */ + .stopmode = true, /* SIRCSTEN */ + .lowpower = true, /* SIRCLPEN */ + .locked = false, /* LK */ + }, + .firc = + { + .range = SCG_FIRC_RANGE_48M, /* RANGE */ + .div1 = SCG_ASYNC_CLOCK_DIV_BY_1, /* FIRCDIV1 */ + .div2 = SCG_ASYNC_CLOCK_DIV_BY_1, /* FIRCDIV2 */ + .initialize = true, /* Initialize */ + .stopmode = false, /* */ + .lowpower = false, /* */ + .regulator = true, /* FIRCREGOFF */ + .locked = false, /* LK */ + }, + .sosc = + { + .mode = SCG_SOSC_MONITOR_DISABLE, /* SOSCCM */ + .gain = SCG_SOSC_GAIN_LOW, /* HGO */ + .range = SCG_SOSC_RANGE_MID, /* RANGE */ + .extref = SCG_SOSC_REF_OSC, /* EREFS */ + .div1 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SOSCDIV1 */ + .div2 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SOSCDIV2 */ + .initialize = true, /* Initialize */ + .stopmode = false, /* */ + .lowpower = false, /* */ + .locked = false, /* LK */ + }, + .spll = + { + .mode = SCG_SPLL_MONITOR_DISABLE, /* SPLLCM */ + .div1 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SPLLDIV1 */ + .div2 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SPLLDIV2 */ + .prediv = 1, /* PREDIV */ + .mult = 40, /* MULT */ + .src = 0, /* SOURCE */ + .initialize = true, /* Initialize */ + .stopmode = false, /* */ + .locked = false, /* LK */ + }, + .rtc = + { + .initialize = true, /* Initialize */ + .clkin = 0 /* RTC_CLKIN */ + }, + .clockout = + { + .source = SCG_CLOCKOUT_SRC_FIRC, /* SCG CLKOUTSEL */ + .initialize = true, /* Initialize */ + }, + .clockmode = + { + .rccr = /* RCCR - Run Clock Control Register */ + { + .src = SCG_SYSTEM_CLOCK_SRC_SYS_PLL, /* SCS */ + .divslow = 3, /* DIVSLOW, range 1..16 */ + .divbus = 2, /* DIVBUS, range 1..16 */ + .divcore = 2 /* DIVCORE, range 1..16 */ + }, + .vccr = /* VCCR - VLPR Clock Control Register */ + { + .src = SCG_SYSTEM_CLOCK_SRC_SIRC, /* SCS */ + .divslow = 4, /* DIVSLOW, range 1..16 */ + .divbus = 1, /* DIVBUS, range 1..16 */ + .divcore = 2 /* DIVCORE, range 1..16 */ + }, + .hccr = + { + .src = SCG_SYSTEM_CLOCK_SRC_SYS_PLL, /* SCS */ + .divslow = 3, /* DIVSLOW, range 1..16 */ + .divbus = 2, /* DIVBUS, range 1..16 */ + .divcore = 2 /* DIVCORE, range 1..16 */ + }, + + /* .altclk */ + + .initialize = true, /* Initialize */ + }, + }, + .sim = + { + .clockout = /* Clock Out configuration. */ + { + .source = SIM_CLKOUT_SEL_SYSTEM_SCG_CLKOUT, /* CLKOUTSEL */ + .divider = 1, /* CLKOUTDIV, range 1..8 */ + .initialize = true, /* Initialize */ + .enable = false, /* CLKOUTEN */ + }, + .lpoclk = /* Low Power Clock configuration. */ + { + .rtc_source = SIM_RTCCLK_SEL_SOSCDIV1_CLK, /* RTCCLKSEL */ + .lpo_source = SIM_LPO_CLK_SEL_LPO_128K, /* LPOCLKSEL */ + .initialize = true, /* Initialize */ + .lpo32k = true, /* LPO32KCLKEN */ + .lpo1k = true, /* LPO1KCLKEN */ + }, + .tclk = /* TCLK CLOCK configuration. */ + { + .tclkfreq[0] = 0, /* TCLK0 */ + .tclkfreq[1] = 0, /* TCLK1 */ + .tclkfreq[2] = 0, /* TCLK2 */ + .initialize = true, /* Initialize */ + }, + .platgate = /* Platform Gate Clock configuration. */ + { + .initialize = true, /* Initialize */ + .mscm = true, /* CGCMSCM */ + .mpu = true, /* CGCMPU */ + .dma = true, /* CGCDMA */ + .erm = true, /* CGCERM */ + .eim = true, /* CGCEIM */ + }, + .traceclk = /* Debug trace Clock Configuration. */ + { + .source = CLOCK_TRACE_SRC_CORE_CLK, /* TRACECLK_SEL */ + .divider = 1, /* TRACEDIV, range 1..8 */ + .initialize = true, /* Initialize */ + .enable = true, /* TRACEDIVEN */ + .fraction = false, /* TRACEFRAC */ + }, +#ifdef CONFIG_S32K1XX_HAVE_QSPI + .qspirefclk = /* Quad Spi Internal Reference Clock Gating. */ + { + .refclk = false, /* Qspi reference clock gating */ + }, +#endif + }, + .pcc = + { + .count = NUM_OF_PERIPHERAL_CLOCKS_0, /* Number peripheral clock configurations */ + .pclks = g_peripheral_clockconfig0 /* Peripheral clock configurations */ + }, + .pmc = + { + .lpoclk = /* Low Power Clock configuration. */ + { + .trim = 0, /* Trimming value for LPO */ + .initialize = true, /* Initialize */ + .enable = true, /* Enable/disable LPO */ + }, + } +}; diff --git a/boards/nxp/rddrone-uavcan146/src/init.c b/boards/nxp/rddrone-uavcan146/src/init.c new file mode 100644 index 000000000000..edaa10813103 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/init.c @@ -0,0 +1,94 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_appinit.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "board_config.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef OK +# define OK 0 +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initialization logic and the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +int board_app_initialize(uintptr_t arg) +{ +#ifdef CONFIG_BOARD_LATE_INITIALIZE + /* Board initialization already performed by board_late_initialize() */ + + return OK; +#else + /* Perform board-specific initialization */ + + return s32k1xx_bringup(); +#endif +} diff --git a/boards/nxp/rddrone-uavcan146/src/periphclocks.c b/boards/nxp/rddrone-uavcan146/src/periphclocks.c new file mode 100644 index 000000000000..7c9bdfdb21f2 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/periphclocks.c @@ -0,0 +1,186 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_periphclks.c + * + * Copyright (C) 2019 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. + * + * Most of the settings within this file derives from NXP sample code for + * the S32K1XX MCUs. That sample code has this licensing information: + * + * Copyright (c) 2013 - 2015, Freescale Semiconductor, Inc. + * Copyright 2016-2018 NXP + * All rights reserved. + * + * THIS SOFTWARE IS PROVIDED BY NXP "AS IS" AND ANY EXPRESSED 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 NXP OR ITS 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 "s32k1xx_periphclocks.h" +#include "board_config.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Each S32K1XX board must provide the following initialized structure. + * This is needed to establish the initial peripheral clocking. + */ + +const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { + { + .clkname = ADC0_CLK, + .clkgate = true, + .clksrc = CLK_SRC_FIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = ADC1_CLK, + .clkgate = true, + .clksrc = CLK_SRC_FIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPI2C0_CLK, + .clkgate = true, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPSPI0_CLK, + .clkgate = true, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPSPI1_CLK, + .clkgate = false, + .clksrc = CLK_SRC_FIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPSPI2_CLK, + .clkgate = false, + .clksrc = CLK_SRC_FIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPTMR0_CLK, + .clkgate = true, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPUART0_CLK, + .clkgate = true, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPUART1_CLK, + .clkgate = true, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = LPUART2_CLK, + .clkgate = false, + .clksrc = CLK_SRC_SIRC, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = PORTA_CLK, + .clkgate = true, + .clksrc = CLK_SRC_OFF, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = PORTB_CLK, + .clkgate = true, + .clksrc = CLK_SRC_OFF, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = PORTC_CLK, + .clkgate = true, + .clksrc = CLK_SRC_OFF, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = PORTD_CLK, + .clkgate = true, + .clksrc = CLK_SRC_OFF, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + }, + { + .clkname = PORTE_CLK, + .clkgate = true, + .clksrc = CLK_SRC_OFF, + .frac = MULTIPLY_BY_ONE, + .divider = 1, + } +}; diff --git a/boards/nxp/rddrone-uavcan146/src/spi.c b/boards/nxp/rddrone-uavcan146/src/spi.c new file mode 100644 index 000000000000..5629b941515e --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/spi.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_spi.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Ivan Ucherdzhiev + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" + +#include "s32k1xx_config.h" +#include "s32k1xx_lpspi.h" +#include "s32k1xx_pin.h" +#include "board_config.h" + +#if defined(CONFIG_S32K1XX_LPSPI0) || defined(CONFIG_S32K1XX_LPSPI1) || \ + defined(CONFIG_S32K1XX_LPSPI2) + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k1xx_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the RDDRONE-UAVCAN146 + * board. + * + ****************************************************************************/ + +void weak_function s32k1xx_spidev_initialize(void) +{ +#ifdef CONFIG_S32K1XX_LPSPI0 + s32k1xx_pinconfig(PIN_LPSPI0_PCS); + +#ifdef CONFIG_SPI_DRIVER + struct spi_dev_s *g_lpspi0; + g_lpspi0 = s32k1xx_lpspibus_initialize(0); + + if (!g_lpspi0) { + spierr("ERROR: [boot] FAILED to initialize LPSPI0\n"); + } + + spi_register(g_lpspi0, 0); +#endif +#endif + +#ifdef CONFIG_S32K1XX_LPSPI1 + s32k1xx_pinconfig(PIN_LPSPI1_PCS); + +#ifdef CONFIG_SPI_DRIVER + struct spi_dev_s *g_lpspi1; + g_lpspi1 = s32k1xx_lpspibus_initialize(1); + + if (!g_lpspi1) { + spierr("ERROR: [boot] FAILED to initialize LPSPI1\n"); + } + + spi_register(g_lpspi1, 1); +#endif +#endif + +#ifdef CONFIG_S32K1XX_LPSPI2 + s32k1xx_pinconfig(PIN_LPSPI2_PCS); + +#ifdef CONFIG_SPI_DRIVER + struct spi_dev_s *g_lpspi2; + g_lpspi2 = s32k1xx_lpspibus_initialize(2); + + if (!g_lpspi2) { + spierr("ERROR: [boot] FAILED to initialize LPSPI2\n"); + } + + spi_register(g_lpspi2, 2); +#endif +#endif +} + +/**************************************************************************** + * Name: s32k1xx_lpspi0/1/2select and s32k1xx_lpspi0/1/2status + * + * Description: + * The external functions, s32k1xx_lpspi0/1/2select and + * s32k1xx_lpspi0/1/2status must be provided by board-specific logic. + * They are implementations of the select and status methods of the SPI + * interface defined by struct spi_ops_s (see include/nuttx/spi/spi.h). + * All other methods (including s32k1xx_lpspibus_initialize()) are provided + * by common logic. To use this common SPI logic on your board: + * + * 1. Provide logic in s32k1xx_boardinitialize() to configure SPI chip + * select pins. + * 2. Provide s32k1xx_lpspi0/1/2select() and s32k1xx_lpspi0/1/2status() + * functions in your board-specific logic. These functions will perform + * chip selection and status operations using GPIOs in the way your + * board is configured. + * 3. Add a calls to s32k1xx_lpspibus_initialize() in your low level + * application initialization logic + * 4. The handle returned by s32k1xx_lpspibus_initialize() may then be used + * to bind the SPI driver to higher level logic (e.g., calling + * mmcsd_spislotinitialize(), for example, will bind the SPI driver to + * the SPI MMC/SD driver). + * + ****************************************************************************/ + +#ifdef CONFIG_S32K1XX_LPSPI0 +void s32k1xx_lpspi0select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, + selected ? "assert" : "de-assert"); + + s32k1xx_gpiowrite(PIN_LPSPI0_PCS, !selected); +} + +uint8_t s32k1xx_lpspi0status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return 0; +} +#endif + +#ifdef CONFIG_S32K1XX_LPSPI1 +void s32k1xx_lpspi1select(FAR struct spi_dev_s *dev, + uint32_t devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, + selected ? "assert" : "de-assert"); + + s32k1xx_gpiowrite(PIN_LPSPI1_PCS, !selected); +} + +uint8_t s32k1xx_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return 0; +} +#endif + +#ifdef CONFIG_S32K1XX_LPSPI2 +void s32k1xx_lpspi2select(FAR struct spi_dev_s *dev, + uint32_t devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, + selected ? "assert" : "de-assert"); + + s32k1xx_gpiowrite(PIN_LPSPI2_PCS, !selected); +} + +uint8_t s32k1xx_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return 0; +} +#endif + +#endif /* CONFIG_S32K1XX_LPSPI0 || CONFIG_S32K1XX_LPSPI01 || CONFIG_S32K1XX_LPSPI2 */ diff --git a/boards/nxp/rddrone-uavcan146/src/userleds.c b/boards/nxp/rddrone-uavcan146/src/userleds.c new file mode 100644 index 000000000000..76bdccf8f354 --- /dev/null +++ b/boards/nxp/rddrone-uavcan146/src/userleds.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * boards/arm/s32k1xx/rddrone-uavcan146/src/s32k1xx_userleds.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +#include "up_arch.h" +#include "up_internal.h" + +#include "s32k1xx_pin.h" +#include "board_config.h" + +#include + +#ifndef CONFIG_ARCH_LEDS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_userled_initialize + ****************************************************************************/ + +void board_userled_initialize(void) +{ + /* Configure LED GPIOs for output */ + + s32k1xx_pinconfig(GPIO_LED_R); + s32k1xx_pinconfig(GPIO_LED_G); + s32k1xx_pinconfig(GPIO_LED_B); +} + +/**************************************************************************** + * Name: board_userled + ****************************************************************************/ + +void board_userled(int led, bool ledon) +{ + uint32_t ledcfg; + + if (led == BOARD_LED_R) { + ledcfg = GPIO_LED_R; + + } else if (led == BOARD_LED_G) { + ledcfg = GPIO_LED_G; + + } else if (led == BOARD_LED_B) { + ledcfg = GPIO_LED_B; + + } else { + return; + } + + s32k1xx_gpiowrite(ledcfg, ledon); /* High illuminates */ +} + +/**************************************************************************** + * Name: board_userled_all + ****************************************************************************/ + +void board_userled_all(uint8_t ledset) +{ + /* Low illuminates */ + + s32k1xx_gpiowrite(GPIO_LED_R, (ledset & BOARD_LED_R_BIT) != 0); + s32k1xx_gpiowrite(GPIO_LED_G, (ledset & BOARD_LED_G_BIT) != 0); + s32k1xx_gpiowrite(GPIO_LED_B, (ledset & BOARD_LED_B_BIT) != 0); +} + +#endif /* !CONFIG_ARCH_LEDS */ From fc9894d20f0106d8baf06868279967196e0531a2 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 1 Feb 2020 15:25:55 -0800 Subject: [PATCH 4/4] Add nxp_rddrone-uavcan146_default to CI --- .ci/Jenkinsfile-compile | 1 + 1 file changed, 1 insertion(+) diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index dd0806e15227..f4bafb9a6707 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -38,6 +38,7 @@ pipeline { "mro_x21_default", "mro_ctrl-zero-f7_default", "mro_x21-777_default", "nxp_fmuk66-v3_default", "nxp_fmurt1062-v1_default", + "nxp_rddrone-uavcan146_default", "omnibus_f4sd_default", "px4_fmu-v2_default", "px4_fmu-v2_fixedwing", "px4_fmu-v2_lpe", "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v2_test", "px4_fmu-v3_default",