fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

NXP MR-CANHUBK3 Support

Browse Source
This commit is contained in:
Peter van der Perk
2022-12-06 06:49:28 -05:00
committed by David Sidrane
parent 7f01e3962f
commit 45244e610f
69 changed files with 8961 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/src/px4/nxp/s32k34x/CMakeLists.txt
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# 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(../s32k3xx/adc adc)
add_subdirectory(../s32k3xx/board_reset board_reset)
add_subdirectory(../s32k3xx/board_critmon board_critmon)
add_subdirectory(../s32k3xx/led_pwm led_pwm)
add_subdirectory(../s32k3xx/hrt hrt)
add_subdirectory(../s32k3xx/io_pins io_pins)
add_subdirectory(../s32k3xx/tone_alarm tone_alarm)
add_subdirectory(../s32k3xx/version version)
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/adc.h
+35
View File
@@ -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 "../../../s32k3xx/include/px4_arch/adc.h"
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/hw_description.h
+36
View File
@@ -0,0 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../../../s32k3xx/include/px4_arch/hw_description.h"
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/i2c_hw_description.h
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2021 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 <px4_arch/hw_description.h>
#include <px4_platform_common/i2c.h>
#if defined(CONFIG_I2C)
static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
{
px4_i2c_bus_t ret{};
ret.bus = bus;
ret.is_external = false;
return ret;
}
static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
{
px4_i2c_bus_t ret{};
ret.bus = bus;
ret.is_external = true;
return ret;
}
#endif // CONFIG_I2C
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer.h
+35
View File
@@ -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 "../../../s32k3xx/include/px4_arch/io_timer.h"
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer_hw_description.h
+36
View File
@@ -0,0 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../../../s32k3xx/include/px4_arch/io_timer_hw_description.h"
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/micro_hal.h
+124
View File
@@ -0,0 +1,124 @@
/****************************************************************************
*
* 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 "../../../nxp_common/include/px4_arch/micro_hal.h"
__BEGIN_DECLS
#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_NXPS32K344
// 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 <chip.h>
#include <s32k3xx_pin.h>
#include <s32k3xx_lpspi.h>
#include <s32k3xx_lpi2c.h>
#include <s32k3xx_flexcan.h>
//#include <s32k3xx_uid.h>
/* s32k3xx 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)
/* bus_num is zero based on s32k3xx and must be translated from the legacy one based */
#define PX4_BUS_OFFSET 1 /* s32k3xx buses are 0 based and adjustment is needed */
#define px4_spibus_initialize(bus_num_1based) s32k3xx_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
#define px4_i2cbus_initialize(bus_num_1based) s32k3xx_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
#define px4_i2cbus_uninitialize(pdev) s32k3xx_i2cbus_uninitialize(pdev)
#define px4_arch_configgpio(pinset) s32k3xx_pinconfig(pinset)
#define px4_arch_unconfiggpio(pinset)
#define px4_arch_gpioread(pinset) s32k3xx_gpioread(pinset)
#define px4_arch_gpiowrite(pinset, value) s32k3xx_gpiowrite(pinset, value)
/* s32k3xx_gpiosetevent is added at PX4 level */
int s32k3xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) s32k3xx_gpiosetevent(pinset,r,f,e,fp,a)
/* CAN bootloader usage */
#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000)
#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000)
#define crc_HiLOC S32K1XX_CAN0_RXIMR27
#define crc_LoLOC S32K1XX_CAN0_RXIMR28
#define signature_LOC S32K1XX_CAN0_RXIMR29
#define bus_speed_LOC S32K1XX_CAN0_RXIMR30
#define node_id_LOC S32K1XX_CAN0_RXIMR31
__END_DECLS
platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/spi_hw_description.h
+86
View File
@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2021 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 "../../../s32k3xx/include/px4_arch/spi_hw_description.h"
constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS])
{
const bool nuttx_enabled_spi_buses[] = {
#ifdef CONFIG_S32K3XX_LPSPI0
true,
#else
false,
#endif
#ifdef CONFIG_S32K3XX_LPSPI1
true,
#else
false,
#endif
#ifdef CONFIG_S32K3XX_LPSPI2
true,
#else
false,
#endif
#ifdef CONFIG_S32K3XX_LPSPI3
true,
#else
false,
#endif
#ifdef CONFIG_S32K3XX_LPSPI4
true,
#else
false,
#endif
#ifdef CONFIG_S32K3XX_LPSPI5
true,
#else
false,
#endif
};
for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) {
bool found_bus = false;
for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) {
if (spi_busses_conf[j].bus == (int)i + 1) {
found_bus = true;
}
}
// Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured
constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_S32K3XX_LPSPIn)");
}
return false;
}
platforms/nuttx/src/px4/nxp/s32k3xx/adc/CMakeLists.txt
+36
View File
@@ -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
)
platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp
+195
View File
@@ -0,0 +1,195 @@
/****************************************************************************
*
* 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 <board_config.h>
#include <drivers/drv_adc.h>
#include <drivers/drv_hrt.h>
#include <px4_arch/adc.h>
#include <nuttx/analog/adc.h>
#include <hardware/s32k1xx_sim.h>
//todo S32K add ADC fior now steal the kinetis one
#include <kinetis.h>
#include <hardware/kinetis_adc.h>
#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;
}
float px4_arch_adc_reference_v()
{
return BOARD_ADC_POS_REF_V; // TODO: provide true vref
}
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
}
platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/CMakeLists.txt
+36
View File
@@ -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
)
platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/board_critmon.c
+66
View File
@@ -0,0 +1,66 @@
/************************************************************************************
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <nuttx/config.h>
#include <arch/board/board.h>
#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 */
platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# 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.cpp
)
# up_systemreset
if (NOT DEFINED CONFIG_BUILD_FLAT)
target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
else()
target_link_libraries(arch_board_reset PRIVATE nuttx_arch)
endif()
platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp
+95
View File
@@ -0,0 +1,95 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
* Author: @author Peter van der Perk <peter.vanderperk@nxp.com>
* David Sidrane <david_s5@nscdg.com>
*
* 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.cpp
* Implementation of kinetis based Board RESET API
*/
#include <px4_platform_common/px4_config.h>
#include <errno.h>
#include <nuttx/board.h>
#include <hardware/s32k3xx_mc_me.h>
#ifdef CONFIG_BOARDCTL_RESET
static int board_reset_enter_bootloader()
{
putreg32(MC_ME_MODE_CONF_FUNC_RST, S32K3XX_MC_ME_MODE_CONF);
putreg32(MC_ME_MODE_UPD, S32K3XX_MC_ME_MODE_UPD);
putreg32(0x5AF0, S32K3XX_MC_ME_CTL_KEY);
putreg32(~0x5AF0, S32K3XX_MC_ME_CTL_KEY);
while (getreg32(S32K3XX_MC_ME_MODE_UPD) & MC_ME_MODE_UPD);
return OK;
}
/****************************************************************************
* 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)
{
if (status == 1) {
board_reset_enter_bootloader();
}
#if defined(BOARD_HAS_ON_RESET)
board_on_reset(status);
#endif
up_systemreset();
return 0;
}
#endif /* CONFIG_BOARDCTL_RESET */
platforms/nuttx/src/px4/nxp/s32k3xx/hrt/CMakeLists.txt
+37
View File
@@ -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
platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c
+521
View File
@@ -0,0 +1,521 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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: Peter van der Perk <peter.vanderperk@nxp.com>
* David Sidrane <david_s5@nscdg.com>
*
* High-resolution timer callouts and timekeeping.
*
* This driver uses the S32K3 System Time Module STM
*
* Note that really, this could use systick too, but that's
* monopolised by NuttX and stealing it would just be awkward.
*/
#include <px4_platform_common/px4_config.h>
#include <systemlib/px4_macros.h>
#include <lib/perf/perf_counter.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include "hardware/s32k3xx_stm.h"
#ifdef CONFIG_DEBUG_HRT
# define hrtinfo _info
#else
# define hrtinfo(x...)
#endif
#ifdef HRT_TIMER
#define HRT_TIMER_FREQ 1000000 //FXOSC 16Mhz / 2 / 8
#define HRT_TIMER_VECTOR S32K3XX_IRQ_STM0
/**
* 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 UINT32_MAX
/*
* 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))
/*
* Specific registers and bits used by HRT sub-functions
*/
#define STATUS_HRT STM_CIR_CIF
/*
* Queue of callout entries.
*/
static struct sq_queue_s callout_queue;
/* latency baseline (last compare value applied) */
static uint32_t latency_baseline;
/* timer count at interrupt (for latency purposes) */
static uint32_t latency_actual;
/* latency histogram */
const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
/* timer-specific functions */
static void hrt_tim_init(void);
static int hrt_tim_isr(int irq, void *context, 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);
/**
* Initialize the timer we are going to use.
*/
static void hrt_tim_init(void)
{
/* FTM clock should be configured in s32k1xx_periphclocks.c */
/* claim our interrupt vector */
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL);
/* Reset the timer counter */
putreg32(0, S32K3XX_STM0_CNT);
/* Timer enable and prescaler 8 */
putreg32(STM_CR_TEN | STM_CR_CPS(8), S32K3XX_STM0_CR);
/* Channel 0 HRT */
putreg32(1000, S32K3XX_STM0_CMP0);
putreg32(STM_CCR_CEN, S32K3XX_STM0_CCR0);
/* enable interrupts */
up_enable_irq(HRT_TIMER_VECTOR);
}
/**
* 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 = getreg32(S32K3XX_STM0_CNT);
/* copy interrupt status */
uint32_t status = getreg32(S32K3XX_STM0_CIR0);
/* was this a timer tick? */
if (status & STATUS_HRT) {
putreg32(STATUS_HRT, S32K3XX_STM0_CIR0);
/* 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 = 0;
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 = getreg32(S32K3XX_STM0_CNT);
/*
* 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;
}
/**
* Store the absolute time in an interrupt-safe fashion
*/
void
hrt_store_absolute_time(volatile hrt_abstime *t)
{
irqstate_t flags = px4_enter_critical_section();
*t = hrt_absolute_time();
px4_leave_critical_section(flags);
}
/**
* Initialize the high-resolution timing module.
*/
void
hrt_init(void)
{
sq_init(&callout_queue);
hrt_tim_init();
}
/**
* 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 */
latency_baseline = deadline & 0xffffffff;
putreg32(latency_baseline, S32K3XX_STM0_CMP0);
}
static void
hrt_latency_update(void)
{
uint32_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 */
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/adc.h
+39
View File
@@ -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 <board_config.h>
#define SYSTEM_ADC_BASE 0 // not used on s32k1xx
#include <px4_platform/adc.h>
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h
+254
View File
@@ -0,0 +1,254 @@
/****************************************************************************
*
* 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 <stdint.h>
#include <s32k3xx_pin.h>
#include <px4_platform_common/constexpr_util.h>
/*
* Timers
*/
namespace Timer
{
enum Timer {
EMIOS0 = 0,
EMIOS1,
EMIOS2,
};
enum Channel {
Channel0 = 0,
Channel1,
Channel2,
Channel3,
Channel4,
Channel5,
Channel6,
Channel7,
};
struct TimerChannel {
Timer timer;
Channel channel;
};
}
static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
{
switch (timer) {
case Timer::EMIOS0: return S32K3XX_EMIOS0_BASE;
case Timer::EMIOS1: return S32K3XX_EMIOS1_BASE;
case Timer::EMIOS2: return S32K3XX_EMIOS2_BASE;
}
return 0;
}
/*
* GPIO
*/
namespace GPIO
{
enum Port {
PortInvalid = 0,
PortA,
PortB,
PortC,
PortD,
PortE,
};
enum Pin {
Pin0 = 0,
Pin1,
Pin2,
Pin3,
Pin4,
Pin5,
Pin6,
Pin7,
Pin8,
Pin9,
Pin10,
Pin11,
Pin12,
Pin13,
Pin14,
Pin15,
Pin16,
Pin17,
Pin18,
Pin19,
Pin20,
Pin21,
Pin22,
Pin23,
Pin24,
Pin25,
Pin26,
Pin27,
Pin28,
Pin29,
Pin30,
Pin31,
};
struct GPIOPin {
Port port;
Pin pin;
uint16_t imcr;
};
}
static inline constexpr uint32_t getGPIOPort(GPIO::Port port)
{
switch (port) {
case GPIO::PortA: return PIN_PORTA;
case GPIO::PortB: return PIN_PORTB;
case GPIO::PortC: return PIN_PORTC;
case GPIO::PortD: return PIN_PORTD;
case GPIO::PortE: return PIN_PORTE;
default: break;
}
return 0;
}
static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
{
switch (pin) {
case GPIO::Pin0: return PIN0;
case GPIO::Pin1: return PIN1;
case GPIO::Pin2: return PIN2;
case GPIO::Pin3: return PIN3;
case GPIO::Pin4: return PIN4;
case GPIO::Pin5: return PIN5;
case GPIO::Pin6: return PIN6;
case GPIO::Pin7: return PIN7;
case GPIO::Pin8: return PIN8;
case GPIO::Pin9: return PIN9;
case GPIO::Pin10: return PIN10;
case GPIO::Pin11: return PIN11;
case GPIO::Pin12: return PIN12;
case GPIO::Pin13: return PIN13;
case GPIO::Pin14: return PIN14;
case GPIO::Pin15: return PIN15;
case GPIO::Pin16: return PIN16;
case GPIO::Pin17: return PIN17;
case GPIO::Pin18: return PIN18;
case GPIO::Pin19: return PIN19;
case GPIO::Pin20: return PIN20;
case GPIO::Pin21: return PIN21;
case GPIO::Pin22: return PIN22;
case GPIO::Pin23: return PIN23;
case GPIO::Pin24: return PIN24;
case GPIO::Pin25: return PIN25;
case GPIO::Pin26: return PIN26;
case GPIO::Pin27: return PIN27;
case GPIO::Pin28: return PIN28;
case GPIO::Pin29: return PIN29;
case GPIO::Pin30: return PIN30;
case GPIO::Pin31: return PIN31;
}
return 0;
}
static inline constexpr uint32_t getIMCR(uint16_t imcr)
{
return IMCR(imcr);
}
namespace SPI
{
// SPI3 is ignored only used for FS26
enum class Bus {
SPI0 = 1,
SPI1,
SPI2,
SPI3,
SPI4,
SPI5,
};
using CS = GPIO::GPIOPin;
using DRDY = GPIO::GPIOPin;
struct bus_device_external_cfg_t {
CS cs_gpio;
DRDY drdy_gpio;
};
} // namespace SPI
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/i2c_hw_description.h
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2021 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 <px4_arch/hw_description.h>
#include <px4_platform_common/i2c.h>
#if defined(CONFIG_I2C)
static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
{
px4_i2c_bus_t ret{};
ret.bus = bus;
ret.is_external = false;
return ret;
}
static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
{
px4_i2c_bus_t ret{};
ret.bus = bus;
ret.is_external = true;
return ret;
}
#endif // CONFIG_I2C
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* 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 <px4_platform_common/px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <drivers/drv_hrt.h>
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 1
#define MAX_TIMER_IO_CHANNELS 8
#define MAX_LED_TIMERS 2
#define MAX_TIMER_LED_CHANNELS 3
#define IO_TIMER_ALL_MODES_CHANNELS 0
typedef enum io_timer_channel_mode_t {
IOTimerChanMode_NotUsed = 0,
IOTimerChanMode_PWMOut = 1,
IOTimerChanMode_PWMIn = 2,
IOTimerChanMode_Capture = 3,
IOTimerChanMode_OneShot = 4,
IOTimerChanMode_Trigger = 5,
IOTimerChanMode_Dshot = 6,
IOTimerChanMode_LED = 7,
IOTimerChanMode_PPS = 8,
IOTimerChanMode_Other = 9,
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_0_3; /* IRQ number */
uint32_t vectorno_4_7; /* IRQ number */
uint32_t vectorno_8_11; /* IRQ number */
uint32_t vectorno_12_15; /* IRQ number */
uint32_t vectorno_16_19; /* IRQ number */
uint32_t vectorno_20_23; /* IRQ number */
} io_timers_t;
typedef struct io_timers_channel_mapping_element_t {
uint32_t first_channel_index;
uint32_t channel_count;
uint32_t lowest_timer_channel;
uint32_t channel_count_including_gaps;
} io_timers_channel_mapping_element_t;
/* mapping for each io_timers to timer_io_channels */
typedef struct io_timers_channel_mapping_t {
io_timers_channel_mapping_element_t element[MAX_IO_TIMERS];
} io_timers_channel_mapping_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 io_timers_channel_mapping_t io_timers_channel_mapping;
__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 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, io_timer_channel_mode_t mode);
__EXPORT int io_timer_set_pwm_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 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_allocate_channel(unsigned channel, io_timer_channel_mode_t mode);
__EXPORT int io_timer_unallocate_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(unsigned channel_mask);
/**
* Reserve a timer
* @return 0 on success (if not used yet, or already set to the mode)
*/
__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode);
__EXPORT int io_timer_unallocate_timer(unsigned timer);
/**
* Returns the pin configuration for a specific channel, to be used as GPIO output.
* 0 is returned if the channel is not valid.
*/
__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel);
/**
* Returns the pin configuration for a specific channel, to be used as PWM input.
* 0 is returned if the channel is not valid.
*/
__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel);
__END_DECLS
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer_hw_description.h
+139
View File
@@ -0,0 +1,139 @@
/****************************************************************************
*
* 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 <px4_arch/io_timer.h>
#include <px4_arch/hw_description.h>
#include <px4_platform_common/constexpr_util.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform/io_timer_init.h>
#include "hardware/s32k3xx_emios.h"
/* TO DO: This mess only supports FTM 0-5, not 6-7 (only available on S32K148).
* It's also hard to check if it is correct. *Should* work for S32K146, not confirmed for other S32K1XX MCUs. */
static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
Timer::TimerChannel timer, uint32_t pinmux)
{
timer_io_channels_t ret{};
ret.gpio_in = pinmux;
ret.gpio_out = pinmux;
ret.timer_channel = (int)timer.channel + 1;
// find timer index
ret.timer_index = 0xff;
const uint32_t timer_base = timerBaseRegister(timer.timer);
for (int i = 0; i < MAX_IO_TIMERS; ++i) {
if (io_timers_conf[i].base == timer_base) {
ret.timer_index = i;
break;
}
}
constexpr_assert(ret.timer_index != 0xff, "Timer not found");
return ret;
}
/* eMIOS has 24 channels and 4 channels per IRQ, io_timers_t can only specify one vector
* However we can follow the pattern using the intitial IRQ vector
* Ch0 - Ch3 = vectorno
* Ch4 - Ch7 = vectorno - 1
* Ch8 - Ch11 = vectorno - 2
* Ch12 - Ch15 = vectorno - 3
* Ch16 - Ch19 = vectorno - 4
* Ch20 - Ch23 = vectorno - 5
*/
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer)
{
bool nuttx_config_timer_enabled = false;
io_timers_t ret{};
switch (timer) {
case Timer::EMIOS0:
ret.base = S32K3XX_EMIOS0_BASE;
ret.clock_register = 0;
ret.clock_bit = 0;
ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS0_0_3;
ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS0_4_7;
ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS0_8_11;
ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS0_12_15;
ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS0_16_19;
ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS0_20_23;
#ifdef CONFIG_S32K3XX_EMIOS0
nuttx_config_timer_enabled = true;
#endif
break;
case Timer::EMIOS1:
ret.base = S32K3XX_EMIOS1_BASE;
ret.clock_register = 0;
ret.clock_bit = 0;
ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS1_0_3;
ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS1_4_7;
ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS1_8_11;
ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS1_12_15;
ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS1_16_19;
ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS1_20_23;
#ifdef CONFIG_S32K3XX_EMIOS1
nuttx_config_timer_enabled = true;
#endif
break;
case Timer::EMIOS2:
ret.base = S32K3XX_EMIOS2_BASE;
ret.clock_register = 0;
ret.clock_bit = 0;
ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS2_0_3;
ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS2_4_7;
ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS2_8_11;
ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS2_12_15;
ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS2_16_19;
ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS2_20_23;
#ifdef CONFIG_S32K3XX_EMIOS2
nuttx_config_timer_enabled = true;
#endif
break;
}
// This is not strictly required, but for consistency let's make sure NuttX timers are disabled
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (S32K1XX_FTMx)");
return ret;
}
platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h
+134
View File
@@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (C) 2020 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 <px4_arch/hw_description.h>
#include <px4_platform_common/spi.h>
#include "s32k3xx_config.h"
#include "s32k3xx_lpspi.h"
#include "s32k3xx_pin.h"
static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
{
px4_spi_bus_device_t ret{};
ret.cs_gpio = getGPIOPort(cs_gpio.port) | getGPIOPin(cs_gpio.pin) | (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE);
if (drdy_gpio.port != GPIO::PortInvalid) {
ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | getIMCR(drdy_gpio.imcr) |
(GPIO_PULLUP | PIN_INT_BOTH);
}
if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external)
ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid);
} else { // it's a NuttX device (e.g. SPIDEV_FLASH(0))
ret.devid = devid;
}
ret.devtype_driver = PX4_SPI_DEV_ID(devid);
return ret;
}
static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices,
GPIO::GPIOPin power_enable = {})
{
px4_spi_bus_t ret{};
ret.requires_locking = false;
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
ret.devices[i] = devices.devices[i];
if (ret.devices[i].cs_gpio != 0) {
if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
int same_devices_count = 0;
for (int j = 0; j < i; ++j) {
if (ret.devices[j].cs_gpio != 0) {
same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
}
}
// increment the 2. LSB byte to allow multiple devices of the same type
ret.devices[i].devid |= same_devices_count << 8;
} else {
// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
ret.requires_locking = true;
}
}
}
ret.bus = (int)bus;
ret.is_external = false;
if (power_enable.port != GPIO::PortInvalid) {
ret.power_enable_gpio = getGPIOPort(power_enable.port) | getGPIOPin(power_enable.pin) |
(GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE);
}
return ret;
}
// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments
struct bus_device_external_cfg_array_t {
SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES];
};
static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices)
{
px4_spi_bus_t ret{};
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
if (devices.devices[i].cs_gpio.port == GPIO::PortInvalid) {
break;
}
ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio);
}
ret.bus = (int)bus;
ret.is_external = true;
ret.requires_locking = false; // external buses are never accessed by NuttX drivers
return ret;
}
static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
{
SPI::bus_device_external_cfg_t ret{};
ret.cs_gpio = cs_gpio;
ret.drdy_gpio = drdy_gpio;
return ret;
}
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/CMakeLists.txt
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# 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
s32k3xx_pinirq.c
)
target_link_libraries(arch_io_pins PRIVATE drivers_board)
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/input_capture.c
+409
View File
@@ -0,0 +1,409 @@
/****************************************************************************
*
* 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 <px4_platform_common/px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_input_capture.h>
#include <px4_arch/io_timer.h>
#include "s32k3xx_pin.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, S32K1XX_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].latency) {
channel_stats[chan_index].latency = (isrs_rcnt - capture);
}
channel_stats[chan_index].edges++;
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
uint32_t overflow = 0;//_REG32(timer, S32K1XX_FTM_CNSC_OFFSET(chan->timer_channel - 1)) & FTM_CNSC_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 {
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) {
//FIXME
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) {
//FIXME
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, S32K1XX_FTM_CNSC_OFFSET(timer_io_channels[channel].timer_channel - 1));
rvalue &= (FTM_CNSC_MSB | FTM_CNSC_MSA);
switch (rvalue) {
case (FTM_CNSC_MSA):
*edge = Rising;
break;
case (FTM_CNSC_MSB):
*edge = Falling;
break;
case (FTM_CNSC_MSB | FTM_CNSC_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_CNSC_MSA;
break;
case Falling:
edge_bits = FTM_CNSC_MSB;
break;
case Both:
edge_bits = (FTM_CNSC_MSB | FTM_CNSC_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, S32K1XX_FTM_CNSC_OFFSET(timer_io_channels[channel].timer_channel - 1));
rvalue &= (FTM_CNSC_MSB | FTM_CNSC_MSA);
rvalue |= edge_bits;
_REG32(timer, S32K1XX_FTM_CNSC_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;
}
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/io_timer.c
+900
View File
@@ -0,0 +1,900 @@
/****************************************************************************
*
* 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 io_timer.c
*
* Servo driver supporting PWM servos connected to S32K3XX EMIOS blocks.
*/
#include <px4_platform_common/px4_config.h>
#include <systemlib/px4_macros.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
#include "s32k3xx_pin.h"
#include "hardware/s32k3xx_emios.h"
static int io_timer_handler0(int irq, void *context, void *arg);
static int io_timer_handler1(int irq, void *context, void *arg);
static int io_timer_handler2(int irq, void *context, void *arg);
static int io_timer_handler3(int irq, void *context, void *arg);
static int io_timer_handler4(int irq, void *context, void *arg);
static int io_timer_handler5(int irq, void *context, void *arg);
static int io_timer_handler6(int irq, void *context, void *arg);
static int io_timer_handler7(int irq, void *context, void *arg);
/* The FTM pre-scalers are limited to divide by 2^n where n={1-7}.
*
* All EMIOS blocks have their clock sources set to the core_clk
* which should generate an 160 / 80 = 2 MHz clock.
*/
#define BOARD_PWM_PRESCALER 20
#if !defined(BOARD_PWM_FREQ)
#define BOARD_PWM_FREQ 160000000 / BOARD_PWM_PRESCALER
#endif
#if !defined(BOARD_ONESHOT_FREQ)
#define BOARD_ONESHOT_FREQ 160000000
#endif
#define FTM_SRC_CLOCK_FREQ 160000000
#define MAX_CHANNELS_PER_TIMER 24
#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 rMCR(_tmr) REG(_tmr, S32K3XX_EMIOS_MCR_OFFSET)
#define rGLAG(_tmr) REG(_tmr, S32K3XX_EMIOS_GFLAG_OFFSET)
#define rOUDIS(_tmr) REG(_tmr, S32K3XX_EMIOS_OUDIS_OFFSET)
#define rUCDIS(_tmr) REG(_tmr, S32K3XX_EMIOS_UCDIS_OFFSET)
#define rA(_tmr,c) REG(_tmr, S32K3XX_EMIOS_A_OFFSET(c))
#define rB(_tmr,c) REG(_tmr, S32K3XX_EMIOS_B_OFFSET(c))
#define rCNT(_tmr,c) REG(_tmr, S32K3XX_EMIOS_CNT_OFFSET(c))
#define rC(_tmr,c) REG(_tmr, S32K3XX_EMIOS_C_OFFSET(c))
#define rS(_tmr,c) REG(_tmr, S32K3XX_EMIOS_S_OFFSET(c))
#define rALTA(_tmr,c) REG(_tmr, S32K3XX_EMIOS_ALTA_OFFSET(c))
#define rC2(_tmr,c) REG(_tmr, S32K3XX_EMIOS_C2_OFFSET(c))
// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { };
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)
{
#if 0
/* 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 */
uint32_t first_channel_index = io_timers_channel_mapping.element[timer_index].first_channel_index;
uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer_index].channel_count;
for (unsigned chan_index = first_channel_index; chan_index < 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, S32K1XX_FTM_CNV_OFFSET(chan_index)));
}
}
/* Did it set again during call out? */
if (rSTATUS(timer_index) & chan) {
/* Error - we had a second edge before we serviced the first */
io_timer_channel_stats[chan_index].overflows++;
}
}
#endif
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);
}
int io_timer_handler4(int irq, void *context, void *arg)
{
return io_timer_handler(4);
}
int io_timer_handler5(int irq, void *context, void *arg)
{
return io_timer_handler(5);
}
int io_timer_handler6(int irq, void *context, void *arg)
{
return io_timer_handler(6);
}
int io_timer_handler7(int irq, void *context, void *arg)
{
return io_timer_handler(7);
}
static inline int validate_timer_index(unsigned timer)
{
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
}
int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode)
{
int ret = -EINVAL;
if (validate_timer_index(timer) == 0) {
// check if timer is unused or already set to the mode we want
if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) {
timer_allocations[timer] = mode;
ret = 0;
} else {
ret = -EBUSY;
}
}
return ret;
}
int io_timer_unallocate_timer(unsigned timer)
{
int ret = -EINVAL;
if (validate_timer_index(timer) == 0) {
timer_allocations[timer] = IOTimerChanMode_NotUsed;
ret = 0;
}
return ret;
}
static inline int channels_timer(unsigned channel)
{
return timer_io_channels[channel].timer_index;
}
static uint32_t get_timer_channels(unsigned timer)
{
uint32_t channels = 0;
static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
if (validate_timer_index(timer) < 0) {
return channels;
} else {
if (channels_cache[timer] == 0) {
/* Gather the channel bits that belong to the timer */
uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
channels |= 1 << chan_index;
}
/* cache them */
channels_cache[timer] = channels;
}
}
return channels_cache[timer];
}
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;
}
uint32_t io_timer_channel_get_gpio_output(unsigned channel)
{
if (io_timer_validate_channel_index(channel) != 0) {
return 0;
}
return (timer_io_channels[channel].gpio_out & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) | GPIO_HIGHDRIVE;
}
uint32_t io_timer_channel_get_as_pwm_input(unsigned channel)
{
if (io_timer_validate_channel_index(channel) != 0) {
return 0;
}
return timer_io_channels[channel].gpio_in;
}
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;
}
__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
{
irqstate_t flags = px4_enter_critical_section();
int existing_mode = io_timer_get_channel_mode(channel);
int ret = -EBUSY;
if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
channel_allocations[mode] |= bit;
ret = 0;
}
px4_leave_critical_section(flags);
return ret;
}
int io_timer_unallocate_channel(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;
}
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 = io_timer_allocate_channel(channel, mode);
}
}
return rv;
}
static int timer_set_rate(unsigned channel, unsigned rate)
{
irqstate_t flags = px4_enter_critical_section();
int prediv = 1;
if (rate < 500) {
rC(channels_timer(channel), channel) = (rC(channels_timer(channel), channel) & ~EMIOS_C_UCPRE_MASK)
| EMIOS_C_UCPRE_DIV4;
prediv = 4;
} else {
rC(channels_timer(channel), channel) = (rC(channels_timer(channel), channel) & ~EMIOS_C_UCPRE_MASK)
| EMIOS_C_UCPRE_DIV1;
}
/* configure the timer to update at the desired rate */
rB(channels_timer(channel), channel) = ((BOARD_PWM_FREQ / prediv) / rate) - 1;
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 channel)
{
irqstate_t flags = px4_enter_critical_section();
px4_leave_critical_section(flags);
}
void io_timer_trigger(unsigned channel_mask)
{
int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask;
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], S32K1XX_FTM_SYNC_OFFSET) |= FTM_SYNC;
}
px4_leave_critical_section(flags);
}
int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
{
if (validate_timer_index(timer) != 0) {
return -EINVAL;
}
io_timer_channel_mode_t previous_mode = timer_allocations[timer];
int rv = io_timer_allocate_timer(timer, mode);
/* Do this only once per timer */
if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) {
irqstate_t flags = px4_enter_critical_section();
/* disable and configure the timer */
rMCR(timer) &= ~EMIOS_MCR_GPREN;
/* Set EMIOS prescaler div by 1 clk_freq is 160Mhz */
rMCR(timer) |= EMIOS_MCR_GTBE;
rMCR(timer) |= EMIOS_MCR_GPRE(BOARD_PWM_PRESCALER - 1);
/* Set to run in debug mode */
rMCR(timer) |= EMIOS_MCR_FRZ;
/* Chn23 acts as EMIOS Global counter bus */
rC(timer, 23) |= EMIOS_C_FREN;
rC(timer, 23) |= EMIOS_C_UCPRE_DIV1;
/* Default period hence clk_freq is 160Mhz */
rA(timer, 23) = EMIOS_A(1);
/* Offset at start */
rCNT(timer, 23) = 0;
/* Master mode */
rC(timer, 23) |= EMIOS_C_MODE_MC_UPCNT_CLRSTRT_INTCLK;
/* Prescaler enable */
rC(timer, 23) |= EMIOS_C_UCPREN;
/* Start EMIOS */
rMCR(timer) |= EMIOS_MCR_GPREN;
/*
* 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.
*/
xcpt_t handler;
switch (timer) {
case 0: handler = io_timer_handler0; break;
case 1: handler = io_timer_handler1; break;
case 2: handler = io_timer_handler2; break;
case 3: handler = io_timer_handler3; break;
case 4: handler = io_timer_handler4; break;
case 5: handler = io_timer_handler5; break;
case 6: handler = io_timer_handler6; break;
case 7: handler = io_timer_handler7; break;
default:
handler = NULL;
rv = -EINVAL;
break;
}
if (handler) {
irq_attach(io_timers[timer].vectorno_0_3, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_0_3);
irq_attach(io_timers[timer].vectorno_4_7, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_4_7);
irq_attach(io_timers[timer].vectorno_8_11, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_8_11);
irq_attach(io_timers[timer].vectorno_12_15, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_12_15);
irq_attach(io_timers[timer].vectorno_16_19, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_16_19);
irq_attach(io_timers[timer].vectorno_20_23, handler, NULL);
up_enable_irq(io_timers[timer].vectorno_20_23);
}
px4_leave_critical_section(flags);
}
return rv;
}
int io_timer_set_pwm_rate(unsigned timer, unsigned rate)
{
/* Change only a timer that is owned by pwm or one shot */
if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) {
return -EINVAL;
}
/* Get the channel bits that belong to the timer and are in PWM or OneShot mode */
uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) |
io_timer_get_mode_channels(IOTimerChanMode_PWMOut));
/* Request to use OneShot ?*/
if (PWM_RATE_ONESHOT == rate) {
/* Request to use OneShot
*/
int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot);
/* Did the allocation change */
if (changed_channels) {
io_timer_set_oneshot_mode(timer);
}
} else {
/* Request to use PWM
*/
int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut);
if (changed_channels) {
io_timer_set_PWM_mode(timer);
}
timer_set_rate(timer, rate);
}
return OK;
}
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context)
{
if (io_timer_validate_channel_index(channel) != 0) {
return -EINVAL;
}
uint32_t gpio = timer_io_channels[channel].gpio_in;
uint32_t clearbits = 0; //FIXME
uint32_t setbits = 0;
(void)setbits;
(void)clearbits;
/* figure out the GPIO config first */
switch (mode) {
case IOTimerChanMode_OneShot:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_Trigger:
setbits = 0; //FIXME
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
break;
case IOTimerChanMode_NotUsed:
setbits = 0;
break;
default:
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config
int previous_mode = io_timer_get_channel_mode(channel);
int rv = allocate_channel(channel, mode);
unsigned timer = channels_timer(channel);
if (rv == 0) {
/* Try to reserve & initialize the timer - it will only do it once */
rv = io_timer_init_timer(timer, mode);
if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) {
/* free the channel if it was not used before */
io_timer_unallocate_channel(channel);
}
}
/* Valid channel should now be reserved in new mode */
if (rv == 0) {
/* Set up IO */
if (gpio) {
px4_arch_configgpio(gpio);
}
/* configure the channel */
uint32_t chan = timer_io_channels[channel].timer_channel - 1;
//FIXME use setbits/clearbits for different modes
/* Reset default */
rC(timer, chan) = 0;
/* Enable output update */
rOUDIS(timer) &= ~EMIOS_OUDIS_OU(chan);
/* OPWFMB mode initialization */
rC(timer, chan) |= EMIOS_C_BSL_INTCNT;
/* Duty cycle */
rA(timer, chan) = EMIOS_A(0U); //Start at zero
/* Period cycle */
rB(timer, chan) = EMIOS_B(32768U);
/* PWM mode */
rC(timer, chan) = ((rC(timer, chan) & ~EMIOS_C_MODE_MASK) | EMIOS_C_MODE_OPWFMB_BMATCH);
/* Edge polarity active low */
//rC(timer,chan) |= EMIOS_C_EDPOL;
/* Internal prescaler
* Div by 1, source prescaled lcok
*/
rC2(timer, chan) |= ((rC2(timer, chan) & ~EMIOS_C2_UCEXTPRE_MASK) | EMIOS_C2_UCEXTPRE(0));
//rC2(timer,chan) |= EMIOS_C2_UCPRECLK;
io_timer_set_PWM_mode(chan);
timer_set_rate(chan, 50);
rC(timer, chan) |= EMIOS_C_UCPREN;
channel_handlers[channel].callback = channel_handler;
channel_handlers[channel].context = context;
}
px4_leave_critical_section(flags);
return rv;
}
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
{
switch (mode) {
case IOTimerChanMode_NotUsed:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_OneShot:
case IOTimerChanMode_Trigger:
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
if (state) {
}
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];
}
irqstate_t flags = px4_enter_critical_section();
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);
if ((state &&
(mode == IOTimerChanMode_PWMOut ||
mode == IOTimerChanMode_OneShot ||
mode == IOTimerChanMode_Trigger))) {
rOUDIS(timer) &= ~EMIOS_OUDIS_OU(chan);
} else {
rOUDIS(timer) |= EMIOS_OUDIS_OU(chan);
}
}
}
}
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 {
//FIXME why multiple by 2
/* configure the channel */
irqstate_t flags = px4_enter_critical_section();
rA(channels_timer(channel), timer_io_channels[channel].timer_channel - 1) = EMIOS_A(value * 2);
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)) {
/* Read rALTA to fetch AS2 shadow register */
value = (rALTA(channels_timer(channel), timer_io_channels[channel].timer_channel - 1) & EMIOS_A_MASK) / 2;
}
}
return value;
}
uint32_t io_timer_get_group(unsigned timer)
{
return get_timer_channels(timer);
}
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_servo.c
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* 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 S32K1XX FlexTimer blocks.
*/
#include <px4_platform_common/px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
#include "s32k3xx_pin.h"
int up_pwm_servo_set(unsigned channel, uint16_t value)
{
return io_timer_set_ccr(channel, value);
}
uint16_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) |
io_timer_get_mode_channels(IOTimerChanMode_OneShot);
// 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_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel);
io_timer_unallocate_channel(channel);
current &= ~(1 << channel);
}
}
/* Now allocate the new set */
int ret_val = OK;
int channels_init_mask = 0;
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
/* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */
ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
channel_mask &= ~(1 << channel);
if (OK == ret_val) {
channels_init_mask |= 1 << channel;
} else if (ret_val == -EBUSY) {
/* either timer or channel already used - this is not fatal */
ret_val = 0;
}
}
}
return ret_val == OK ? channels_init_mask : ret_val;
}
void up_pwm_servo_deinit(uint32_t channel_mask)
{
/* disable the timers */
up_pwm_servo_arm(false, channel_mask);
}
int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)
{
if (io_timer_validate_channel_index(channel) < 0) {
return ERROR;
}
/* Allow a rate of 0 to enter oneshot mode */
if (rate != 0) {
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1) {
return -ERANGE;
}
if (rate > 10000) {
return -ERANGE;
}
}
return io_timer_set_pwm_rate(channel, rate);
}
void up_pwm_update(unsigned channel_mask)
{
io_timer_trigger(channel_mask);
}
uint32_t up_pwm_servo_get_rate_group(unsigned group)
{
/* only return the set of channels in the group which we own */
return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) |
io_timer_get_mode_channels(IOTimerChanMode_OneShot)) &
io_timer_get_group(group);
}
void
up_pwm_servo_arm(bool armed, uint32_t channel_mask)
{
io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
}
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c
+115
View File
@@ -0,0 +1,115 @@
/****************************************************************************
*
* 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 <px4_platform_common/px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_trigger.h>
#include <px4_arch/io_timer.h>
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 */
int ret_val = OK;
int channels_init_mask = 0;
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
channel_mask &= ~(1 << channel);
if (OK == ret_val) {
channels_init_mask |= 1 << channel;
} else if (ret_val == -EBUSY) {
/* either timer or channel already used - this is not fatal */
ret_val = 0;
}
}
}
/* Enable the timers */
if (ret_val == OK) {
up_pwm_trigger_arm(true);
}
return ret_val == OK ? channels_init_mask : ret_val;
}
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);
}
platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c
+92
View File
@@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (C) 2020 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 <px4_platform_common/px4_config.h>
#include <systemlib/px4_macros.h>
#include <arch/board/board.h>
#include <errno.h>
#include <s32k3xx_pin.h>
#include <hardware/s32k3xx_siul2.h>
#include <hardware/s32k3xx_wkpu.h>
/****************************************************************************
* Name: s32k3xx_gpiosetevent
*
* Description:
* Sets/clears GPIO based event and interrupt triggers.
*
* Input Parameters:
* - pinset: gpio pin configuration
* - rising/falling edge: enables
* - event: generate event when set
* - func: when non-NULL, generate interrupt
* - arg: Argument passed to the interrupt callback
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure indicating the
* nature of the failure.
*
****************************************************************************/
#if defined(CONFIG_S32K3XX_GPIOIRQ)
int s32k3xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge,
bool event, xcpt_t func, void *arg)
{
int ret = -ENOSYS;
s32k3xx_pinconfig(pinset);
if (func == NULL) {
s32k3xx_pinirqdisable(pinset);
ret = s32k3xx_pinirqattach(pinset, NULL, NULL);
} else {
ret = s32k3xx_pinirqattach(pinset, func, arg);
pinset &= ~_PIN_INT_MASK;
if (risingedge) {
pinset |= PIN_INT_RISING;
}
if (fallingedge) {
pinset |= PIN_INT_FALLING;
}
s32k3xx_pinirqenable(pinset);
}
return ret;
}
#endif /* CONFIG_S32K3XX_GPIOIRQ */
platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/CMakeLists.txt
+36
View File
@@ -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
)
platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp
+350
View File
@@ -0,0 +1,350 @@
/****************************************************************************
*
* 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 <px4_platform_common/px4_config.h>
#include <systemlib/px4_macros.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
#include "s32k1xx_pin.h"
#include "hardware/s32k1xx_pcc.h"
#include "hardware/s32k1xx_ftm.h"
#if defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM)
#define FTM_SRC_CLOCK_FREQ 8000000
#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, S32K1XX_FTM_SC_OFFSET)
#define rCNT(_tmr) REG(_tmr, S32K1XX_FTM_CNT_OFFSET)
#define rMOD(_tmr) REG(_tmr, S32K1XX_FTM_MOD_OFFSET)
#define rC0SC(_tmr) REG(_tmr, S32K1XX_FTM_C0SC_OFFSET)
#define rC0V(_tmr) REG(_tmr, S32K1XX_FTM_C0V_OFFSET)
#define rC1SC(_tmr) REG(_tmr, S32K1XX_FTM_C1SC_OFFSET)
#define rC1V(_tmr) REG(_tmr, S32K1XX_FTM_C1V_OFFSET)
#define rC2SC(_tmr) REG(_tmr, S32K1XX_FTM_C2SC_OFFSET)
#define rC2V(_tmr) REG(_tmr, S32K1XX_FTM_C2V_OFFSET)
#define rC3SC(_tmr) REG(_tmr, S32K1XX_FTM_C3SC_OFFSET)
#define rC3V(_tmr) REG(_tmr, S32K1XX_FTM_C3V_OFFSET)
#define rC4SC(_tmr) REG(_tmr, S32K1XX_FTM_C4SC_OFFSET)
#define rC4V(_tmr) REG(_tmr, S32K1XX_FTM_C4V_OFFSET)
#define rC5SC(_tmr) REG(_tmr, S32K1XX_FTM_C5SC_OFFSET)
#define rC5V(_tmr) REG(_tmr, S32K1XX_FTM_C5V_OFFSET)
#define rC6SC(_tmr) REG(_tmr, S32K1XX_FTM_C6SC_OFFSET)
#define rC6V(_tmr) REG(_tmr, S32K1XX_FTM_C6V_OFFSET)
#define rC7SC(_tmr) REG(_tmr, S32K1XX_FTM_C7SC_OFFSET)
#define rC7V(_tmr) REG(_tmr, S32K1XX_FTM_C7V_OFFSET)
#define rCNTIN(_tmr) REG(_tmr, S32K1XX_FTM_CNTIN_OFFSET)
#define rSTATUS(_tmr) REG(_tmr, S32K1XX_FTM_STATUS_OFFSET)
#define rMODE(_tmr) REG(_tmr, S32K1XX_FTM_MODE_OFFSET)
#define rSYNC(_tmr) REG(_tmr, S32K1XX_FTM_SYNC_OFFSET)
#define rOUTINIT(_tmr) REG(_tmr, S32K1XX_FTM_OUTINIT_OFFSET)
#define rOUTMASK(_tmr) REG(_tmr, S32K1XX_FTM_OUTMASK_OFFSET)
#define rCOMBINE(_tmr) REG(_tmr, S32K1XX_FTM_COMBINE_OFFSET)
#define rDEADTIME(_tmr) REG(_tmr, S32K1XX_FTM_DEADTIME_OFFSET)
#define rEXTTRIG(_tmr) REG(_tmr, S32K1XX_FTM_EXTTRIG_OFFSET)
#define rPOL(_tmr) REG(_tmr, S32K1XX_FTM_POL_OFFSET)
#define rFMS(_tmr) REG(_tmr, S32K1XX_FTM_FMS_OFFSET)
#define rFILTER(_tmr) REG(_tmr, S32K1XX_FTM_FILTER_OFFSET)
#define rFLTCTRL(_tmr) REG(_tmr, S32K1XX_FTM_FLTCTRL_OFFSET)
#define rQDCTRL(_tmr) REG(_tmr, S32K1XX_FTM_QDCTRL_OFFSET)
#define rCONF(_tmr) REG(_tmr, S32K1XX_FTM_CONF_OFFSET)
#define rFLTPOL(_tmr) REG(_tmr, S32K1XX_FTM_FLTPOL_OFFSET)
#define rSYNCONF(_tmr) REG(_tmr, S32K1XX_FTM_SYNCONF_OFFSET)
#define rINVCTRL(_tmr) REG(_tmr, S32K1XX_FTM_INVCTRL_OFFSET)
#define rSWOCTRL(_tmr) REG(_tmr, S32K1XX_FTM_SWOCTRL_OFFSET)
#define rPWMLOAD(_tmr) REG(_tmr, S32K1XX_FTM_PWMLOAD_OFFSET)
#define CnSC_RESET (FTM_CNSC_CHF | FTM_CNSC_CHIE | FTM_CNSC_MSB | FTM_CNSC_MSA | FTM_CNSC_ELSB | FTM_CNSC_ELSA | FTM_CNSC_DMA)
#define CnSC_CAPTURE_INIT (FTM_CNSC_CHIE | FTM_CNSC_ELSB | FTM_CNSC_ELSA) // Both
#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW)
#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_ELSA)
#else
#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_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_DIS;
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, S32K1XX_FTM_CNSC_OFFSET(chan));
rvalue &= ~CnSC_RESET;
rvalue |= CnSC_PWMOUT_INIT;
REG(timer, S32K1XX_FTM_CNSC_OFFSET(chan)) = rvalue;
REG(timer, S32K1XX_FTM_CNV_OFFSET(0)) = 0;
rSC(timer) |= 1 << (FTM_SC_PWMEN_SHIFT + chan);
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, S32K1XX_FTM_CNV_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, S32K1XX_FTM_CNV_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++) {
#if defined(BOARD_HAS_SHARED_PWM_TIMERS)
// Use the io_timer init to mark it initialized
io_timer_init_timer(i, IOTimerChanMode_LED);
#endif
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_DIS;
rCNT(i) = 0;
}
}
}
}
#endif // BOARD_HAS_LED_PWM || BOARD_HAS_UI_LED_PWM
platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt
+36
View File
@@ -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
)
platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/ToneAlarmInterface.cpp
+193
View File
@@ -0,0 +1,193 @@
/****************************************************************************
*
* 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 <drivers/device/device.h>
#include <drivers/drv_tone_alarm.h>
#include <px4_platform_common/defines.h>
#include <systemlib/px4_macros.h>
#include <math.h>
#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.
}
hrt_abstime 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.
irqstate_t flags = enter_critical_section();
hrt_abstime time_started = hrt_absolute_time();
px4_arch_configgpio(GPIO_TONE_ALARM);
leave_critical_section(flags);
return time_started;
}
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 */
platforms/nuttx/src/px4/nxp/s32k3xx/version/CMakeLists.txt
+37
View File
@@ -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
)
platforms/nuttx/src/px4/nxp/s32k3xx/version/board_identity.c
+137
View File
@@ -0,0 +1,137 @@
/****************************************************************************
*
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
* Author: @author David Sidrane <david_s5@nscdg.com>
*
* 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 <px4_platform_common/px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <hardware/s32k3xx_memorymap.h>
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))
#define CHIP_UID 0x1b000040
void board_get_uuid(uuid_byte_t uuid_bytes)
{
uint32_t *chip_uuid = (uint32_t *) CHIP_UID;
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, &current_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;
}
platforms/nuttx/src/px4/nxp/s32k3xx/version/board_mcu_version.c
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
* Author: @author Peter van der Perk <peter.vanderperk@nxp.com>
* David Sidrane <david_s5@nscdg.com>
*
* 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 S32K1xx based SoC version API
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include "arm_internal.h"
#include "hardware/s32k3xx_mcm.h"
#define CHIP_TAG "S32K3XX"
#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1
int board_mcu_version(char *rev, const char **revstr, const char **errata)
{
uint32_t midr1 = getreg32(S32K3XX_SIUL2_MIDR1);
static char chip[sizeof(CHIP_TAG)] = CHIP_TAG;
if ((midr1 & SIUL2_MIDR1_PART_NO_MASK) == SIUL2_MIDR1_PART_NO_S32K344) {
chip[CHIP_TAG_LEN - 1] = '4';
chip[CHIP_TAG_LEN - 2] = '4';
}
*revstr = chip;
*rev = '0' + ((midr1 & SIUL2_MIDR1_MAJOR_MASK) >> SIUL2_MIDR1_MAJOR_SHIFT);
if (errata) {
*errata = NULL;
}
return 0;
}