forked from Archive/PX4-Autopilot
Add nxp imxrt device support
This commit is contained in:
David Sidrane
Daniel Agar
parent
92e75452f5
commit
490fe8256f
|
|
@ -380,6 +380,7 @@ typedef enum PX4_SOC_ARCH_ID_t {
|
|||
PX4_SOC_ARCH_ID_STM32F7 = 0x0002,
|
||||
PX4_SOC_ARCH_ID_KINETISK66 = 0x0003,
|
||||
PX4_SOC_ARCH_ID_SAMV7 = 0x0004,
|
||||
PX4_SOC_ARCH_ID_NXPIMXRT1062 = 0x0005,
|
||||
|
||||
PX4_SOC_ARCH_ID_STM32H7 = 0x0006,
|
||||
|
||||
|
|
|
|||
|
|
@ -178,7 +178,10 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.g
|
|||
# vscode launch.json
|
||||
# FIXME: hack to skip if px4_io-v2 because it's a built within another build (eg px4_fmu-v5)
|
||||
if(NOT PX4_BOARD MATCHES "px4_io-v2")
|
||||
if(CONFIG_ARCH_CHIP_MK66FN2M0VMD18)
|
||||
if(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A)
|
||||
set(DEBUG_DEVICE "MIMXRT1062XXX6A")
|
||||
set(DEBUG_SVD_FILE "MIMXRT1052.svd")
|
||||
elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18)
|
||||
set(DEBUG_DEVICE "MK66FN2M0xxx18")
|
||||
set(DEBUG_SVD_FILE "MK66F18.svd")
|
||||
elseif(CONFIG_ARCH_CHIP_STM32F100C8)
|
||||
|
|
|
|||
|
|
@ -118,6 +118,9 @@ function(px4_os_determine_build_chip)
|
|||
elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "k66")
|
||||
elseif(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "rt106x")
|
||||
else()
|
||||
message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,197 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file adc.cpp
|
||||
*
|
||||
* Driver for the imxrt ADC.
|
||||
*
|
||||
* This is a low-rate driver, designed for sampling things like voltages
|
||||
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
|
||||
*/
|
||||
|
||||
#include <board_config.h>
|
||||
#include <stdint.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <px4_arch/adc.h>
|
||||
|
||||
#include <hardware/imxrt_adc.h>
|
||||
#include <imxrt_periphclks.h>
|
||||
|
||||
typedef uint32_t adc_chan_t;
|
||||
#define ADC_TOTAL_CHANNELS 16
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* ADC register accessors */
|
||||
|
||||
#define REG(base_address, _reg) _REG((base_address) + (_reg))
|
||||
|
||||
#define rHC0(base_address) REG(base_address, IMXRT_ADC_HC0_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC1(base_address) REG(base_address, IMXRT_ADC_HC1_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC2(base_address) REG(base_address, IMXRT_ADC_HC2_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC3(base_address) REG(base_address, IMXRT_ADC_HC3_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC4(base_address) REG(base_address, IMXRT_ADC_HC4_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC5(base_address) REG(base_address, IMXRT_ADC_HC5_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC6(base_address) REG(base_address, IMXRT_ADC_HC6_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHC7(base_address) REG(base_address, IMXRT_ADC_HC7_OFFSET) /* Control register for hardware triggers */
|
||||
#define rHS(base_address) REG(base_address, IMXRT_ADC_HS_OFFSET) /* Status register for HW triggers */
|
||||
#define rR0(base_address) REG(base_address, IMXRT_ADC_R0_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR1(base_address) REG(base_address, IMXRT_ADC_R1_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR2(base_address) REG(base_address, IMXRT_ADC_R2_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR3(base_address) REG(base_address, IMXRT_ADC_R3_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR4(base_address) REG(base_address, IMXRT_ADC_R4_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR5(base_address) REG(base_address, IMXRT_ADC_R5_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR6(base_address) REG(base_address, IMXRT_ADC_R6_OFFSET) /* Data result register for HW triggers */
|
||||
#define rR7(base_address) REG(base_address, IMXRT_ADC_R7_OFFSET) /* Data result register for HW triggers */
|
||||
#define rCFG(base_address) REG(base_address, IMXRT_ADC_CFG_OFFSET) /* Configuration register */
|
||||
#define rGC(base_address) REG(base_address, IMXRT_ADC_GC_OFFSET) /* General control register */
|
||||
#define rGS(base_address) REG(base_address, IMXRT_ADC_GS_OFFSET) /* General status register */
|
||||
#define rCV(base_address) REG(base_address, IMXRT_ADC_CV_OFFSET) /* Compare value register */
|
||||
#define rOFS(base_address) REG(base_address, IMXRT_ADC_OFS_OFFSET) /* Offset correction value register */
|
||||
#define rCAL(base_address) REG(base_address, IMXRT_ADC_CAL_OFFSET) /* Calibration value register */
|
||||
|
||||
|
||||
int px4_arch_adc_init(uint32_t base_address)
|
||||
{
|
||||
static bool once = false;
|
||||
|
||||
if (!once) {
|
||||
|
||||
once = true;
|
||||
|
||||
/* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
imxrt_clockall_adc1();
|
||||
|
||||
rCFG(base_address) = ADC_CFG_ADICLK_IPGDIV2 | ADC_CFG_MODE_12BIT | \
|
||||
ADC_CFG_ADIV_DIV8 | ADC_CFG_ADLSMP | ADC_CFG_ADSTS_6_20 | \
|
||||
ADC_CFG_AVGS_4SMPL | ADC_CFG_OVWREN;
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
/* Clear the CALF and begin the calibration */
|
||||
|
||||
rGS(base_address) = ADC_GS_CALF;
|
||||
rGC(base_address) = ADC_GC_CAL;
|
||||
uint32_t guard = 100;
|
||||
|
||||
while (guard != 0 && (rGS(base_address) & ADC_GC_CAL) == 0) {
|
||||
guard--;
|
||||
usleep(1);
|
||||
}
|
||||
|
||||
while ((rGS(base_address) & ADC_GC_CAL) == ADC_GC_CAL) {
|
||||
|
||||
usleep(100);
|
||||
|
||||
if (rGS(base_address) & ADC_GS_CALF) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if ((rHS(base_address) & ADC_HS_COCO0) == 0) {
|
||||
return -2;
|
||||
}
|
||||
|
||||
if (rGS(base_address) & ADC_GS_CALF) {
|
||||
return -3;
|
||||
}
|
||||
|
||||
/* dummy read to clear COCO of calibration */
|
||||
|
||||
int32_t r = rR0(base_address);
|
||||
UNUSED(r);
|
||||
|
||||
/* kick off a sample and wait for it to complete */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
rGC(base_address) = ADC_GC_AVGE;
|
||||
rHC0(base_address) = 0xd; // VREFSH = internal channel, for ADC self-test, hard connected to VRH internally
|
||||
|
||||
while (!(rHS(base_address) & ADC_HS_COCO0)) {
|
||||
|
||||
/* don't wait for more than 500us, since that means something broke -
|
||||
* should reset here if we see this
|
||||
*/
|
||||
|
||||
if ((hrt_absolute_time() - now) > 500) {
|
||||
return -4;
|
||||
}
|
||||
}
|
||||
|
||||
r = rR0(base_address);
|
||||
} // once
|
||||
|
||||
return 0;
|
||||
}
|
||||
void px4_arch_adc_uninit(uint32_t base_address)
|
||||
{
|
||||
imxrt_clockoff_adc1();
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
|
||||
{
|
||||
|
||||
/* clear any previous COCO0 */
|
||||
|
||||
uint16_t result = rR0(base_address);
|
||||
|
||||
rHC0(base_address) = channel;
|
||||
|
||||
/* wait for the conversion to complete */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
while (!(rHS(base_address) & ADC_HS_COCO0)) {
|
||||
/* don't wait for more than 50us, since that means something broke
|
||||
* should reset here if we see this
|
||||
*/
|
||||
if ((hrt_absolute_time() - now) > 50) {
|
||||
return 0xffff;
|
||||
}
|
||||
}
|
||||
|
||||
/* read the result and clear COCO0 */
|
||||
result = rR0(base_address);
|
||||
return result;
|
||||
}
|
||||
uint32_t px4_arch_adc_temp_sensor_mask()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_dn_fullcount(void)
|
||||
{
|
||||
return 1 << 12; // 12 bit ADC
|
||||
}
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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 */
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_board_hw_info
|
||||
board_hw_rev_ver.c
|
||||
)
|
||||
|
|
@ -0,0 +1,350 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 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_hw_rev_ver.c
|
||||
* Implementation of IMXRT based Board Hardware Revision and Version ID API
|
||||
*/
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <px4_arch/adc.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/board_determine_hw_info.h>
|
||||
#include <stdio.h>
|
||||
#include <board_config.h>
|
||||
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#if defined(BOARD_HAS_HW_VERSIONING)
|
||||
|
||||
# if defined(GPIO_HW_VER_REV_DRIVE)
|
||||
# define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE
|
||||
# define GPIO_HW_VER_DRIVE GPIO_HW_VER_REV_DRIVE
|
||||
# endif
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
static int hw_version = 0;
|
||||
static int hw_revision = 0;
|
||||
static char hw_info[] = HW_INFO_INIT;
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: determin_hw_version
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* This function fist determines if revision and version resistors are in place.
|
||||
* if they it will read the ADC channels and decode the DN to ordinal numbers
|
||||
* that will be returned by board_get_hw_version and board_get_hw_revision API
|
||||
*
|
||||
* This will return OK on success and -1 on not supported
|
||||
*
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int dn_to_ordinal(uint16_t dn)
|
||||
{
|
||||
|
||||
const struct {
|
||||
uint16_t low; // High(n-1) + 1
|
||||
uint16_t high; // Average High(n)+Low(n+1) EX. 1356 = AVRG(1331,1382)
|
||||
} dn2o[] = {
|
||||
// R1(up) R2(down) V min V Max DN Min DN Max
|
||||
{0, 0 }, // 0 No Resistors
|
||||
{1, 579 }, // 1 24.9K 442K 0.166255191 0.44102252 204 553
|
||||
{580, 967 }, // 2 32.4K 174K 0.492349322 0.770203609 605 966
|
||||
{968, 1356}, // 3 38.3K 115K 0.787901749 1.061597759 968 1331
|
||||
{1357, 1756}, // 4 46.4K 84.5K 1.124833577 1.386007306 1382 1738
|
||||
{1757, 2137}, // 5 51.1K 61.9K 1.443393279 1.685367869 1774 2113
|
||||
{2138, 2519}, // 6 61.9K 51.1K 1.758510242 1.974702534 2161 2476
|
||||
{2520, 2919}, // 7 84.5K 46.4K 2.084546498 2.267198261 2562 2842
|
||||
{2920, 3308}, // 8 115K 38.3K 2.437863827 2.57656294 2996 3230
|
||||
{3309, 3699}, // 9 174K 32.4K 2.755223792 2.847933804 3386 3571
|
||||
{3700, 4095}, // 10 442K 24.9K 3.113737849 3.147347506 3827 3946
|
||||
};
|
||||
|
||||
for (unsigned int i = 0; i < arraySize(dn2o); i++) {
|
||||
if (dn >= dn2o[i].low && dn <= dn2o[i].high) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: read_id_dn
|
||||
*
|
||||
* Description:
|
||||
* Read the HW sense set to get a DN of the value formed by
|
||||
* 0 VDD
|
||||
* |
|
||||
* /
|
||||
* \ R1
|
||||
* /
|
||||
* |
|
||||
* +--------------- GPIO_HW_xxx_SENCE | ADC channel N
|
||||
* |
|
||||
* /
|
||||
* \ R2
|
||||
* /
|
||||
* |
|
||||
* |
|
||||
* +--------------- GPIO_HW_xxx_DRIVE or GPIO_HW_VER_REV_DRIVE
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - pointer to receive the dn for the id set
|
||||
* gpio_drive - gpio that is the drive
|
||||
* gpio_sense - gpio that is the sence
|
||||
* adc_channel - the Channel number associated with gpio_sense
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 - Success and id is set
|
||||
* -EIO - FAiled to init or read the ADC
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel)
|
||||
{
|
||||
int rv = -EIO;
|
||||
const unsigned int samples = 16;
|
||||
/*
|
||||
* Step one is there resistors?
|
||||
*
|
||||
* If we set the mid-point of the ladder which is the ADC input to an
|
||||
* output, then whatever state is driven out should be seen by the GPIO
|
||||
* that is on the bottom of the ladder that is switched to an input.
|
||||
* The SENCE line is effectively an output with a high value pullup
|
||||
* resistor on it driving an input through a series resistor with a pull up.
|
||||
* If present the series resistor will form a low pass filter due to stray
|
||||
* capacitance, but this is fine as long as we give it time to settle.
|
||||
*/
|
||||
|
||||
/* Turn the drive lines to digital inputs with No pull up */
|
||||
|
||||
imxrt_config_gpio(_MK_GPIO_INPUT(gpio_drive) & ~IOMUX_PULL_MASK);
|
||||
|
||||
/* Turn the sense lines to digital outputs LOW */
|
||||
|
||||
imxrt_config_gpio(_MK_GPIO_OUTPUT(gpio_sense));
|
||||
|
||||
|
||||
up_udelay(100); /* About 10 TC assuming 485 K */
|
||||
|
||||
/* Read Drive lines while sense are driven low */
|
||||
|
||||
int low = imxrt_gpio_read(_MK_GPIO_INPUT(gpio_drive));
|
||||
|
||||
|
||||
/* Write the sense lines HIGH */
|
||||
|
||||
imxrt_gpio_write(_MK_GPIO_OUTPUT(gpio_sense), 1);
|
||||
|
||||
up_udelay(100); /* About 10 TC assuming 485 K */
|
||||
|
||||
/* Read Drive lines while sense are driven high */
|
||||
|
||||
int high = imxrt_gpio_read(_MK_GPIO_INPUT(gpio_drive));
|
||||
|
||||
/* restore the pins to ANALOG */
|
||||
|
||||
imxrt_config_gpio(gpio_sense);
|
||||
|
||||
/* Turn the drive lines to digital outputs LOW */
|
||||
|
||||
imxrt_config_gpio(gpio_drive ^ GPIO_OUTPUT_SET);
|
||||
|
||||
up_udelay(100); /* About 10 TC assuming 485 K */
|
||||
|
||||
/* Are Resistors in place ?*/
|
||||
|
||||
uint32_t dn_sum = 0;
|
||||
uint16_t dn = 0;
|
||||
#if defined(ON_EVK)
|
||||
|
||||
if (1 || high || low) { // no if
|
||||
#else
|
||||
if ((high ^ low) && low == 0) {
|
||||
#endif
|
||||
/* Yes - Fire up the ADC (it has once control) */
|
||||
|
||||
if (px4_arch_adc_init(HW_REV_VER_ADC_BASE) == OK) {
|
||||
|
||||
/* Read the value */
|
||||
for (unsigned av = 0; av < samples; av++) {
|
||||
dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel);
|
||||
|
||||
if (dn == 0xffff) {
|
||||
break;
|
||||
}
|
||||
|
||||
dn_sum += dn;
|
||||
}
|
||||
|
||||
if (dn != 0xffff) {
|
||||
*id = dn_sum / samples;
|
||||
rv = OK;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* No - No Resistors is ID 0 */
|
||||
*id = 0;
|
||||
rv = OK;
|
||||
}
|
||||
|
||||
/* Turn the drive lines to digital outputs High */
|
||||
|
||||
imxrt_config_gpio(gpio_drive);
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
static int determine_hw_info(int *revision, int *version)
|
||||
{
|
||||
int dn;
|
||||
int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL);
|
||||
|
||||
if (rv == OK) {
|
||||
*revision = dn_to_ordinal(dn);
|
||||
rv = read_id_dn(&dn, GPIO_HW_VER_DRIVE, GPIO_HW_VER_SENSE, ADC_HW_VER_SENSE_CHANNEL);
|
||||
|
||||
if (rv == OK) {
|
||||
*version = dn_to_ordinal(dn);
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_type
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a 0 terminated string defining the HW type.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* a 0 terminated string defining the HW type. This my be a 0 length string ""
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT const char *board_get_hw_type_name()
|
||||
{
|
||||
return (const char *) hw_info;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_version
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a integer HW version
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* An integer value of this boards hardware version.
|
||||
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
|
||||
* A value of 0 is the default for boards supporting the API but not having version.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT int board_get_hw_version()
|
||||
{
|
||||
return hw_version;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_revision
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a integer HW revision
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* An integer value of this boards hardware revision.
|
||||
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
|
||||
* A value of 0 is the default for boards supporting the API but not having revision.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT int board_get_hw_revision()
|
||||
{
|
||||
return hw_revision;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_determine_hw_info
|
||||
*
|
||||
* Description:
|
||||
* Uses the HW revision and version detection added in FMUv5.
|
||||
* See https://docs.google.com/spreadsheets/d/1-n0__BYDedQrc_2NHqBenG1DNepAgnHpSGglke-QQwY
|
||||
* HW REV and VER ID tab.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 - on success or negated errono
|
||||
* 1) The values for integer value of this boards hardware revision is set
|
||||
* 2) The integer value of this boards hardware version is set.
|
||||
* 3) hw_info is populated
|
||||
*
|
||||
* A value of 0 is the default for boards supporting the BOARD_HAS_HW_VERSIONING API.
|
||||
* but not having R1 and R2.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int board_determine_hw_info()
|
||||
{
|
||||
int rv = determine_hw_info(&hw_revision, &hw_version);
|
||||
|
||||
if (rv == OK) {
|
||||
hw_info[HW_INFO_INIT_REV] = board_get_hw_revision() + '0';
|
||||
hw_info[HW_INFO_INIT_VER] = board_get_hw_version() + '0';
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_board_reset
|
||||
board_reset.c
|
||||
)
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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_reset.c
|
||||
* Implementation of IMXRT based Board RESET API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <errno.h>
|
||||
#include <nuttx/board.h>
|
||||
#include <up_arch.h>
|
||||
#include <hardware/imxrt_snvs.h>
|
||||
|
||||
#define PX4_IMXRT_RTC_REBOOT_REG 3 // Must be common with bootloader and:
|
||||
|
||||
#if CONFIG_IMXRT_RTC_MAGIC_REG == PX4_IMXRT_RTC_REBOOT_REG
|
||||
# error CONFIG_IMXRT_RTC_MAGIC_REG can nt have the save value as PX4_IMXRT_RTC_REBOOT_REG
|
||||
#endif
|
||||
|
||||
int board_reset(int status)
|
||||
{
|
||||
up_systemreset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int board_set_bootload_mode(board_reset_e mode)
|
||||
{
|
||||
uint32_t regvalue = 0;
|
||||
|
||||
switch (mode) {
|
||||
case board_reset_normal:
|
||||
case board_reset_extended:
|
||||
break;
|
||||
|
||||
case board_reset_enter_bootloader:
|
||||
regvalue = 0xb007b007;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
putreg32(regvalue, IMXRT_SNVS_LPGPR(PX4_IMXRT_RTC_REBOOT_REG));
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
void board_system_reset(int status)
|
||||
{
|
||||
#if defined(BOARD_HAS_ON_RESET)
|
||||
board_on_reset(status);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_BOARDCTL_RESET
|
||||
board_reset(status);
|
||||
#endif
|
||||
|
||||
while (1);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,875 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_hrt.c
|
||||
* Author: David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* High-resolution timer callouts and timekeeping.
|
||||
*
|
||||
* This can use any GPT timer.
|
||||
*
|
||||
* Note that really, this could use systick too, but that's
|
||||
* monopolised by NuttX and stealing it would just be awkward.
|
||||
*
|
||||
* We don't use the NuttX Kinetis driver per se; rather, we
|
||||
* claim the timer and then drive it directly.
|
||||
*/
|
||||
|
||||
#include <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 "chip.h"
|
||||
#include "hardware/imxrt_gpt.h"
|
||||
#include "imxrt_periphclks.h"
|
||||
|
||||
#undef PPM_DEBUG
|
||||
|
||||
#ifdef CONFIG_DEBUG_HRT
|
||||
# define hrtinfo _info
|
||||
#else
|
||||
# define hrtinfo(x...)
|
||||
#endif
|
||||
|
||||
#define CAT3_(A, B, C) A##B##C
|
||||
#define CAT3(A, B, C) CAT3_(A, B, C)
|
||||
|
||||
#ifdef HRT_TIMER
|
||||
|
||||
#define HRT_TIMER_FREQ 1000000
|
||||
|
||||
/* HRT configuration */
|
||||
|
||||
#define HRT_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */
|
||||
#define HRT_TIMER_BASE CAT3(IMXRT_GPT, HRT_TIMER,_BASE) /* The Base address of the GPT */
|
||||
#define HRT_TIMER_VECTOR CAT(IMXRT_IRQ_GPT, HRT_TIMER) /* The GPT Interrupt vector */
|
||||
|
||||
#if HRT_TIMER == 1
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 2
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */
|
||||
#endif
|
||||
|
||||
#if HRT_TIMER == 1 && defined(CONFIG_IMXRT_GPT1)
|
||||
# error must not set CONFIG_IMXRT_GPT1=y and HRT_TIMER=1
|
||||
#elif HRT_TIMER == 2 && defined(CONFIG_IMXRT_GPT2)
|
||||
# error must not set CONFIG_IMXRT_GPT2=y and HRT_TIMER=2
|
||||
#endif
|
||||
|
||||
/*
|
||||
* HRT clock must be a multiple of 1MHz greater than 1MHz
|
||||
*/
|
||||
#if (HRT_TIMER_CLOCK % HRT_TIMER_FREQ) != 0
|
||||
# error HRT_TIMER_CLOCK must be a multiple of 1MHz
|
||||
#endif
|
||||
#if HRT_TIMER_CLOCK <= HRT_TIMER_FREQ
|
||||
# error HRT_TIMER_CLOCK must be greater than 1MHz
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Minimum/maximum deadlines.
|
||||
*
|
||||
* These are suitable for use with a 32-bit timer/counter clocked
|
||||
* at 1MHz. The high-resolution timer need only guarantee that it
|
||||
* not wrap more than once in the 4294.967296s period for absolute
|
||||
* time to be consistently maintained.
|
||||
*
|
||||
* The minimum deadline must be such that the time taken between
|
||||
* reading a time and writing a deadline to the timer cannot
|
||||
* result in missing the deadline.
|
||||
*/
|
||||
#define HRT_INTERVAL_MIN 5
|
||||
#define HRT_INTERVAL_MAX 4294000000
|
||||
|
||||
/*
|
||||
* Period of the free-running counter, in microseconds.
|
||||
*/
|
||||
#define HRT_COUNTER_PERIOD 4294967296LL
|
||||
|
||||
/*
|
||||
* Scaling factor(s) for the free-running counter; convert an input
|
||||
* in counts to a time in microseconds.
|
||||
*/
|
||||
#define HRT_COUNTER_SCALE(_c) (_c)
|
||||
|
||||
/* Register accessors */
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* Timer register accessors */
|
||||
|
||||
#define REG(_reg) _REG(HRT_TIMER_BASE + (_reg))
|
||||
|
||||
#define rCR REG(IMXRT_GPT_CR_OFFSET)
|
||||
#define rPR REG(IMXRT_GPT_PR_OFFSET)
|
||||
#define rSR REG(IMXRT_GPT_SR_OFFSET)
|
||||
#define rIR REG(IMXRT_GPT_IR_OFFSET)
|
||||
#define rOCR1 REG(IMXRT_GPT_OCR1_OFFSET)
|
||||
#define rOCR2 REG(IMXRT_GPT_OCR2_OFFSET)
|
||||
#define rOCR3 REG(IMXRT_GPT_OCR3_OFFSET)
|
||||
#define rICR1 REG(IMXRT_GPT_ICR1_OFFSET)
|
||||
#define rICR2 REG(IMXRT_GPT_ICR2_OFFSET)
|
||||
#define rCNT REG(IMXRT_GPT_CNT_OFFSET)
|
||||
|
||||
/*
|
||||
* Specific registers and bits used by HRT sub-functions
|
||||
*/
|
||||
|
||||
# define rOCR_HRT CAT(rOCR, HRT_TIMER_CHANNEL) /* GPT Output Compare Register used by HRT */
|
||||
# define STATUS_HRT CAT(GPT_SR_OF, HRT_TIMER_CHANNEL) /* OF Output Compare Flag */
|
||||
# define OFIE_HRT CAT3(GPT_IR_OF, HRT_TIMER_CHANNEL,IE) /* Output Compare Interrupt Enable */
|
||||
|
||||
#if (HRT_TIMER_CHANNEL > 1) || (HRT_TIMER_CHANNEL > 3)
|
||||
# error HRT_TIMER_CHANNEL must be a value between 1 and 3
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Queue of callout entries.
|
||||
*/
|
||||
static struct sq_queue_s callout_queue;
|
||||
|
||||
/* latency baseline (last compare value applied) */
|
||||
static uint32_t latency_baseline;
|
||||
|
||||
/* timer count at interrupt (for latency purposes) */
|
||||
static uint32_t latency_actual;
|
||||
|
||||
/* timer-specific functions */
|
||||
static void hrt_tim_init(void);
|
||||
static int hrt_tim_isr(int irq, void *context, void *args);
|
||||
static void hrt_latency_update(void);
|
||||
|
||||
/* callout list manipulation */
|
||||
static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout,
|
||||
void *arg);
|
||||
static void hrt_call_enter(struct hrt_call *entry);
|
||||
static void hrt_call_reschedule(void);
|
||||
static void hrt_call_invoke(void);
|
||||
|
||||
#if !defined(HRT_PPM_CHANNEL)
|
||||
|
||||
/* When HRT_PPM_CHANNEL is not used provide null operations */
|
||||
|
||||
# define GPT_CR_IM_BOTH 0
|
||||
# define STATUS_PPM 0
|
||||
# define IFIE_PPM 0
|
||||
|
||||
#else
|
||||
|
||||
/* Specific registers and bits used by PPM sub-functions */
|
||||
|
||||
# define rICR_PPM CAT(rICR, HRT_PPM_CHANNEL) /* GPT Input Capture Register used by PPL */
|
||||
# define GPT_CR_IM_BOTH CAT3(GPT_CR_IM, HRT_PPM_CHANNEL, _BOTH) /* GPT Capture mode both */
|
||||
# define STATUS_PPM CAT(GPT_SR_IF, HRT_PPM_CHANNEL) /* IF Input capture Flag */
|
||||
# define IFIE_PPM CAT3(GPT_IR_IF, HRT_PPM_CHANNEL,IE) /* Output Compare Interrupt Enable */
|
||||
|
||||
/* Sanity checking */
|
||||
|
||||
# if (HRT_PPM_CHANNEL != 1) && (HRT_PPM_CHANNEL != 2)
|
||||
# error HRT_PPM_CHANNEL must be a value of 1 or 2
|
||||
# endif
|
||||
|
||||
# if (HRT_PPM_CHANNEL == HRT_TIMER_CHANNEL)
|
||||
# error HRT_PPM_CHANNEL must not be the same as HRT_TIMER_CHANNEL
|
||||
# endif
|
||||
/*
|
||||
* PPM decoder tuning parameters
|
||||
*/
|
||||
# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */
|
||||
# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */
|
||||
# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */
|
||||
# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */
|
||||
# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */
|
||||
|
||||
/* decoded PPM buffer */
|
||||
|
||||
# define PPM_MIN_CHANNELS 5
|
||||
# define PPM_MAX_CHANNELS 20
|
||||
|
||||
/** Number of same-sized frames required to 'lock' */
|
||||
|
||||
# define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */
|
||||
|
||||
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
|
||||
__EXPORT uint16_t ppm_frame_length = 0;
|
||||
__EXPORT unsigned ppm_decoded_channels = 0;
|
||||
__EXPORT uint64_t ppm_last_valid_decode = 0;
|
||||
|
||||
|
||||
# if defined(PPM_DEBUG)
|
||||
|
||||
# define EDGE_BUFFER_COUNT 32
|
||||
|
||||
/* PPM edge history */
|
||||
|
||||
__EXPORT uint32_t ppm_edge_history[EDGE_BUFFER_COUNT];
|
||||
unsigned ppm_edge_next;
|
||||
|
||||
/* PPM pulse history */
|
||||
|
||||
__EXPORT uint32_t ppm_pulse_history[EDGE_BUFFER_COUNT];
|
||||
unsigned ppm_pulse_next;
|
||||
# endif
|
||||
|
||||
static uint32_t ppm_temp_buffer[PPM_MAX_CHANNELS];
|
||||
|
||||
/** PPM decoder state machine */
|
||||
struct {
|
||||
uint32_t last_edge; /**< last capture time */
|
||||
uint32_t last_mark; /**< last significant edge */
|
||||
uint32_t frame_start; /**< the frame width */
|
||||
unsigned next_channel; /**< next channel index */
|
||||
enum {
|
||||
UNSYNCH = 0,
|
||||
ARM,
|
||||
ACTIVE,
|
||||
INACTIVE
|
||||
} phase;
|
||||
} ppm;
|
||||
|
||||
static void hrt_ppm_decode(uint32_t status);
|
||||
#endif /* HRT_PPM_CHANNEL */
|
||||
|
||||
/**
|
||||
* Initialize the timer we are going to use.
|
||||
*/
|
||||
static void hrt_tim_init(void)
|
||||
{
|
||||
/* Enable the Module clock */
|
||||
|
||||
HRT_CLOCK_ALL();
|
||||
|
||||
|
||||
/* claim our interrupt vector */
|
||||
|
||||
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL);
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
rCR = 0;
|
||||
|
||||
rCR = GPT_CR_OM1_DIS | GPT_CR_OM2_DIS | GPT_CR_OM3_DIS |
|
||||
GPT_CR_IM_BOTH | GPT_CR_FRR | GPT_CR_CLKSRC_IPG | GPT_CR_ENMOD;
|
||||
|
||||
/* CLKSRC field is divided by [PRESCALER + 1] */
|
||||
|
||||
rPR = (HRT_TIMER_CLOCK / HRT_TIMER_FREQ) - 1;
|
||||
|
||||
/* set an initial capture a little ways off */
|
||||
|
||||
rOCR_HRT = 1000;
|
||||
|
||||
/* enable interrupts */
|
||||
up_enable_irq(HRT_TIMER_VECTOR);
|
||||
|
||||
rIR = IFIE_PPM | OFIE_HRT;
|
||||
|
||||
/* enable the timer */
|
||||
|
||||
rCR |= GPT_CR_EN;
|
||||
|
||||
}
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/**
|
||||
* Handle the PPM decoder state machine.
|
||||
*/
|
||||
static void hrt_ppm_decode(uint32_t status)
|
||||
{
|
||||
uint32_t count = rICR_PPM;
|
||||
uint32_t width;
|
||||
uint32_t interval;
|
||||
unsigned i;
|
||||
|
||||
/* how long since the last edge? - this handles counter wrapping implicitly. */
|
||||
width = count - ppm.last_edge;
|
||||
|
||||
#if PPM_DEBUG
|
||||
ppm_edge_history[ppm_edge_next++] = width;
|
||||
|
||||
if (ppm_edge_next >= EDGE_BUFFER_COUNT) {
|
||||
ppm_edge_next = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
* if this looks like a start pulse, then push the last set of values
|
||||
* and reset the state machine
|
||||
*/
|
||||
if (width >= PPM_MIN_START) {
|
||||
|
||||
/*
|
||||
* If the number of channels changes unexpectedly, we don't want
|
||||
* to just immediately jump on the new count as it may be a result
|
||||
* of noise or dropped edges. Instead, take a few frames to settle.
|
||||
*/
|
||||
if (ppm.next_channel != ppm_decoded_channels) {
|
||||
static unsigned new_channel_count;
|
||||
static unsigned new_channel_holdoff;
|
||||
|
||||
if (new_channel_count != ppm.next_channel) {
|
||||
/* start the lock counter for the new channel count */
|
||||
new_channel_count = ppm.next_channel;
|
||||
new_channel_holdoff = PPM_CHANNEL_LOCK;
|
||||
|
||||
} else if (new_channel_holdoff > 0) {
|
||||
/* this frame matched the last one, decrement the lock counter */
|
||||
new_channel_holdoff--;
|
||||
|
||||
} else {
|
||||
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
|
||||
ppm_decoded_channels = new_channel_count;
|
||||
new_channel_count = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* frame channel count matches expected, let's use it */
|
||||
if (ppm.next_channel >= PPM_MIN_CHANNELS) {
|
||||
for (i = 0; i < ppm.next_channel; i++) {
|
||||
ppm_buffer[i] = ppm_temp_buffer[i];
|
||||
}
|
||||
|
||||
ppm_last_valid_decode = hrt_absolute_time();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* reset for the next frame */
|
||||
ppm.next_channel = 0;
|
||||
|
||||
/* next edge is the reference for the first channel */
|
||||
ppm.phase = ARM;
|
||||
|
||||
ppm.last_edge = count;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (ppm.phase) {
|
||||
case UNSYNCH:
|
||||
/* we are waiting for a start pulse - nothing useful to do here */
|
||||
break;
|
||||
|
||||
case ARM:
|
||||
|
||||
/* we expect a pulse giving us the first mark */
|
||||
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
|
||||
goto error; /* pulse was too short or too long */
|
||||
}
|
||||
|
||||
/* record the mark timing, expect an inactive edge */
|
||||
ppm.last_mark = ppm.last_edge;
|
||||
|
||||
/* frame length is everything including the start gap */
|
||||
ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
|
||||
ppm.frame_start = ppm.last_edge;
|
||||
ppm.phase = ACTIVE;
|
||||
break;
|
||||
|
||||
case INACTIVE:
|
||||
|
||||
/* we expect a short pulse */
|
||||
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
|
||||
goto error; /* pulse was too short or too long */
|
||||
}
|
||||
|
||||
/* this edge is not interesting, but now we are ready for the next mark */
|
||||
ppm.phase = ACTIVE;
|
||||
break;
|
||||
|
||||
case ACTIVE:
|
||||
/* determine the interval from the last mark */
|
||||
interval = count - ppm.last_mark;
|
||||
ppm.last_mark = count;
|
||||
|
||||
#if PPM_DEBUG
|
||||
ppm_pulse_history[ppm_pulse_next++] = interval;
|
||||
|
||||
if (ppm_pulse_next >= EDGE_BUFFER_COUNT) {
|
||||
ppm_pulse_next = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* if the mark-mark timing is out of bounds, abandon the frame */
|
||||
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) {
|
||||
goto error;
|
||||
}
|
||||
|
||||
/* if we have room to store the value, do so */
|
||||
if (ppm.next_channel < PPM_MAX_CHANNELS) {
|
||||
ppm_temp_buffer[ppm.next_channel++] = interval;
|
||||
}
|
||||
|
||||
ppm.phase = INACTIVE;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
ppm.last_edge = count;
|
||||
return;
|
||||
|
||||
/* the state machine is corrupted; reset it */
|
||||
|
||||
error:
|
||||
/* we don't like the state of the decoder, reset it and try again */
|
||||
ppm.phase = UNSYNCH;
|
||||
ppm_decoded_channels = 0;
|
||||
|
||||
}
|
||||
#endif /* HRT_PPM_CHANNEL */
|
||||
|
||||
/**
|
||||
* Handle the compare interrupt by calling the callout dispatcher
|
||||
* and then re-scheduling the next deadline.
|
||||
*/
|
||||
static int
|
||||
hrt_tim_isr(int irq, void *context, void *arg)
|
||||
{
|
||||
/* grab the timer for latency tracking purposes */
|
||||
|
||||
latency_actual = rCNT;
|
||||
|
||||
/* copy interrupt status */
|
||||
uint32_t status = rSR;
|
||||
|
||||
/* ack the interrupts we just read */
|
||||
|
||||
rSR = status;
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
|
||||
/* was this a PPM edge? */
|
||||
if (status & (STATUS_PPM)) {
|
||||
hrt_ppm_decode(status);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* was this a timer tick? */
|
||||
if (status & STATUS_HRT) {
|
||||
|
||||
/* do latency calculations */
|
||||
hrt_latency_update();
|
||||
|
||||
/* run any callouts that have met their deadline */
|
||||
hrt_call_invoke();
|
||||
|
||||
/* and schedule the next interrupt */
|
||||
hrt_call_reschedule();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* Fetch a never-wrapping absolute time value in microseconds from
|
||||
* some arbitrary epoch shortly after system start.
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_absolute_time(void)
|
||||
{
|
||||
hrt_abstime abstime;
|
||||
uint32_t count;
|
||||
irqstate_t flags;
|
||||
|
||||
/*
|
||||
* Counter state. Marked volatile as they may change
|
||||
* inside this routine but outside the irqsave/restore
|
||||
* pair. Discourage the compiler from moving loads/stores
|
||||
* to these outside of the protected range.
|
||||
*/
|
||||
static volatile hrt_abstime base_time;
|
||||
static volatile uint32_t last_count;
|
||||
|
||||
/* prevent re-entry */
|
||||
flags = px4_enter_critical_section();
|
||||
|
||||
/* get the current counter value */
|
||||
count = rCNT;
|
||||
|
||||
/*
|
||||
* Determine whether the counter has wrapped since the
|
||||
* last time we're called.
|
||||
*
|
||||
* This simple test is sufficient due to the guarantee that
|
||||
* we are always called at least once per counter period.
|
||||
*/
|
||||
if (count < last_count) {
|
||||
base_time += HRT_COUNTER_PERIOD;
|
||||
}
|
||||
|
||||
/* save the count for next time */
|
||||
last_count = count;
|
||||
|
||||
/* compute the current time */
|
||||
abstime = HRT_COUNTER_SCALE(base_time + count);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return abstime;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert a timespec to absolute time
|
||||
*/
|
||||
hrt_abstime
|
||||
ts_to_abstime(const struct timespec *ts)
|
||||
{
|
||||
hrt_abstime result;
|
||||
|
||||
result = (hrt_abstime)(ts->tv_sec) * 1000000;
|
||||
result += ts->tv_nsec / 1000;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert absolute time to a timespec.
|
||||
*/
|
||||
void
|
||||
abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
||||
{
|
||||
ts->tv_sec = abstime / 1000000;
|
||||
abstime -= ts->tv_sec * 1000000;
|
||||
ts->tv_nsec = abstime * 1000;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compare a time value with the current time as atomic operation.
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
hrt_abstime delta = hrt_absolute_time() - *then;
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return delta;
|
||||
}
|
||||
|
||||
/**
|
||||
* Store the absolute time in an interrupt-safe fashion
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_store_absolute_time(volatile hrt_abstime *now)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
hrt_abstime ts = hrt_absolute_time();
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return ts;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the high-resolution timing module.
|
||||
*/
|
||||
void
|
||||
hrt_init(void)
|
||||
{
|
||||
sq_init(&callout_queue);
|
||||
hrt_tim_init();
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/* configure the PPM input pin */
|
||||
px4_arch_configgpio(GPIO_PPM_IN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) after interval has elapsed.
|
||||
*/
|
||||
void
|
||||
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_call_internal(entry,
|
||||
hrt_absolute_time() + delay,
|
||||
0,
|
||||
callout,
|
||||
arg);
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) at calltime.
|
||||
*/
|
||||
void
|
||||
hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_call_internal(entry, calltime, 0, callout, arg);
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) every period.
|
||||
*/
|
||||
void
|
||||
hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_call_internal(entry,
|
||||
hrt_absolute_time() + delay,
|
||||
interval,
|
||||
callout,
|
||||
arg);
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
/* if the entry is currently queued, remove it */
|
||||
/* note that we are using a potentially uninitialized
|
||||
entry->link here, but it is safe as sq_rem() doesn't
|
||||
dereference the passed node unless it is found in the
|
||||
list. So we potentially waste a bit of time searching the
|
||||
queue for the uninitialized entry->link but we don't do
|
||||
anything actually unsafe.
|
||||
*/
|
||||
if (entry->deadline != 0) {
|
||||
sq_rem(&entry->link, &callout_queue);
|
||||
}
|
||||
|
||||
entry->deadline = deadline;
|
||||
entry->period = interval;
|
||||
entry->callout = callout;
|
||||
entry->arg = arg;
|
||||
|
||||
hrt_call_enter(entry);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
/**
|
||||
* If this returns true, the call has been invoked and removed from the callout list.
|
||||
*
|
||||
* Always returns false for repeating callouts.
|
||||
*/
|
||||
bool
|
||||
hrt_called(struct hrt_call *entry)
|
||||
{
|
||||
return (entry->deadline == 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Remove the entry from the callout list.
|
||||
*/
|
||||
void
|
||||
hrt_cancel(struct hrt_call *entry)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
sq_rem(&entry->link, &callout_queue);
|
||||
entry->deadline = 0;
|
||||
|
||||
/* if this is a periodic call being removed by the callout, prevent it from
|
||||
* being re-entered when the callout returns.
|
||||
*/
|
||||
entry->period = 0;
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_call_enter(struct hrt_call *entry)
|
||||
{
|
||||
struct hrt_call *call, *next;
|
||||
|
||||
call = (struct hrt_call *)(void *)sq_peek(&callout_queue);
|
||||
|
||||
if ((call == NULL) || (entry->deadline < call->deadline)) {
|
||||
sq_addfirst(&entry->link, &callout_queue);
|
||||
hrtinfo("call enter at head, reschedule\n");
|
||||
/* we changed the next deadline, reschedule the timer event */
|
||||
hrt_call_reschedule();
|
||||
|
||||
} else {
|
||||
do {
|
||||
next = (struct hrt_call *)(void *)sq_next(&call->link);
|
||||
|
||||
if ((next == NULL) || (entry->deadline < next->deadline)) {
|
||||
hrtinfo("call enter after head\n");
|
||||
sq_addafter(&call->link, &entry->link, &callout_queue);
|
||||
break;
|
||||
}
|
||||
} while ((call = next) != NULL);
|
||||
}
|
||||
|
||||
hrtinfo("scheduled\n");
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_call_invoke(void)
|
||||
{
|
||||
struct hrt_call *call;
|
||||
hrt_abstime deadline;
|
||||
|
||||
while (true) {
|
||||
/* get the current time */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
call = (struct hrt_call *)(void *)sq_peek(&callout_queue);
|
||||
|
||||
if (call == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (call->deadline > now) {
|
||||
break;
|
||||
}
|
||||
|
||||
sq_rem(&call->link, &callout_queue);
|
||||
hrtinfo("call pop\n");
|
||||
|
||||
/* save the intended deadline for periodic calls */
|
||||
deadline = call->deadline;
|
||||
|
||||
/* zero the deadline, as the call has occurred */
|
||||
call->deadline = 0;
|
||||
|
||||
/* invoke the callout (if there is one) */
|
||||
if (call->callout) {
|
||||
hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg);
|
||||
call->callout(call->arg);
|
||||
}
|
||||
|
||||
/* if the callout has a non-zero period, it has to be re-entered */
|
||||
if (call->period != 0) {
|
||||
// re-check call->deadline to allow for
|
||||
// callouts to re-schedule themselves
|
||||
// using hrt_call_delay()
|
||||
if (call->deadline <= now) {
|
||||
call->deadline = deadline + call->period;
|
||||
}
|
||||
|
||||
hrt_call_enter(call);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reschedule the next timer interrupt.
|
||||
*
|
||||
* This routine must be called with interrupts disabled.
|
||||
*/
|
||||
static void
|
||||
hrt_call_reschedule()
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
struct hrt_call *next = (struct hrt_call *)(void *)sq_peek(&callout_queue);
|
||||
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
|
||||
|
||||
/*
|
||||
* Determine what the next deadline will be.
|
||||
*
|
||||
* Note that we ensure that this will be within the counter
|
||||
* period, so that when we truncate all but the low 16 bits
|
||||
* the next time the compare matches it will be the deadline
|
||||
* we want.
|
||||
*
|
||||
* It is important for accurate timekeeping that the compare
|
||||
* interrupt fires sufficiently often that the base_time update in
|
||||
* hrt_absolute_time runs at least once per timer period.
|
||||
*/
|
||||
if (next != NULL) {
|
||||
hrtinfo("entry in queue\n");
|
||||
|
||||
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
|
||||
hrtinfo("pre-expired\n");
|
||||
/* set a minimal deadline so that we call ASAP */
|
||||
deadline = now + HRT_INTERVAL_MIN;
|
||||
|
||||
} else if (next->deadline < deadline) {
|
||||
hrtinfo("due soon\n");
|
||||
deadline = next->deadline;
|
||||
}
|
||||
}
|
||||
|
||||
hrtinfo("schedule for %ul at %ul\n", (unsigned long)(deadline & 0xffffffff), (unsigned long)(now & 0xffffffff));
|
||||
|
||||
/* set the new compare value and remember it for latency tracking */
|
||||
|
||||
rOCR_HRT = latency_baseline = deadline;
|
||||
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_latency_update(void)
|
||||
{
|
||||
uint16_t latency = latency_actual - latency_baseline;
|
||||
unsigned index;
|
||||
|
||||
/* bounded buckets */
|
||||
for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
|
||||
if (latency <= latency_buckets[index]) {
|
||||
latency_counters[index]++;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* catch-all at the end */
|
||||
latency_counters[index]++;
|
||||
}
|
||||
|
||||
void
|
||||
hrt_call_init(struct hrt_call *entry)
|
||||
{
|
||||
memset(entry, 0, sizeof(*entry));
|
||||
}
|
||||
|
||||
void
|
||||
hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
|
||||
{
|
||||
entry->deadline = hrt_absolute_time() + delay;
|
||||
}
|
||||
|
||||
#endif /* HRT_TIMER */
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#if !defined(HW_REV_VER_ADC_BASE)
|
||||
# define HW_REV_VER_ADC_BASE IMXRT_ADC1_BASE
|
||||
#endif
|
||||
|
||||
#if !defined(SYSTEM_ADC_BASE)
|
||||
# define SYSTEM_ADC_BASE IMXRT_ADC1_BASE
|
||||
#endif
|
||||
|
||||
#include <px4_platform/adc.h>
|
||||
|
|
@ -0,0 +1,142 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_io_timer.h
|
||||
*
|
||||
* imxrt-specific PWM output data.
|
||||
*/
|
||||
#include <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 4
|
||||
#define MAX_TIMER_IO_CHANNELS 16
|
||||
|
||||
#define MAX_LED_TIMERS 2
|
||||
#define MAX_TIMER_LED_CHANNELS 6
|
||||
|
||||
#define IO_TIMER_ALL_MODES_CHANNELS 0
|
||||
|
||||
typedef enum io_timer_channel_mode_t {
|
||||
IOTimerChanMode_NotUsed = 0,
|
||||
IOTimerChanMode_PWMOut = 1,
|
||||
IOTimerChanMode_PWMIn = 2,
|
||||
IOTimerChanMode_Capture = 3,
|
||||
IOTimerChanMode_OneShot = 4,
|
||||
IOTimerChanMode_Trigger = 5,
|
||||
IOTimerChanModeSize
|
||||
} io_timer_channel_mode_t;
|
||||
|
||||
typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
|
||||
|
||||
/* array of timers dedicated to PWM in and out and TBD capture use
|
||||
*** Timers are driven from QTIMER3_OUT0
|
||||
*** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz
|
||||
*** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz
|
||||
*** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz
|
||||
*** the resulting PSC will be one and the timer will count at it's clock frequency.
|
||||
*/
|
||||
typedef struct io_timers_t {
|
||||
uint32_t base; /* Base address of the timer */
|
||||
uint32_t clock_register; /* SIM_SCGCn */
|
||||
uint32_t clock_bit; /* SIM_SCGCn bit pos */
|
||||
uint32_t vectorno; /* IRQ number */
|
||||
uint32_t first_channel_index; /* 0 based index in timer_io_channels */
|
||||
uint32_t last_channel_index; /* 0 based index in timer_io_channels */
|
||||
xcpt_t handler;
|
||||
} io_timers_t;
|
||||
|
||||
/* array of channels in logical order */
|
||||
typedef struct timer_io_channels_t {
|
||||
uint32_t gpio_out; /* The timer valn_offset GPIO for PWM */
|
||||
uint32_t gpio_in; /* The timer valn_offset GPIO for Capture */
|
||||
uint8_t timer_index; /* 0 based index in the io_timers_t table */
|
||||
uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */
|
||||
uint8_t sub_module; /* 0 based sub module offset */
|
||||
uint8_t sub_module_bits; /* LDOK and CLDOK bits */
|
||||
} timer_io_channels_t;
|
||||
|
||||
#define SM0 0
|
||||
#define SM1 1
|
||||
#define SM2 2
|
||||
#define SM3 3
|
||||
|
||||
#define PWMA_VAL IMXRT_FLEXPWM_SM0VAL3_OFFSET
|
||||
#define PWMB_VAL IMXRT_FLEXPWM_SM0VAL5_OFFSET
|
||||
|
||||
|
||||
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
|
||||
const timer_io_channels_t *chan,
|
||||
hrt_abstime isrs_time, uint16_t isrs_rcnt,
|
||||
uint16_t capture);
|
||||
|
||||
|
||||
/* supplied by board-specific code */
|
||||
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
|
||||
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
|
||||
__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
|
||||
|
||||
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
|
||||
__EXPORT int io_timer_handler0(int irq, void *context, void *arg);
|
||||
__EXPORT int io_timer_handler1(int irq, void *context, void *arg);
|
||||
__EXPORT int io_timer_handler2(int irq, void *context, void *arg);
|
||||
__EXPORT int io_timer_handler3(int irq, void *context, void *arg);
|
||||
|
||||
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context);
|
||||
|
||||
__EXPORT int io_timer_init_timer(unsigned timer);
|
||||
|
||||
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
|
||||
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
|
||||
io_timer_channel_allocation_t masks);
|
||||
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
|
||||
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
|
||||
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
|
||||
__EXPORT uint32_t io_timer_get_group(unsigned timer);
|
||||
__EXPORT int io_timer_validate_channel_index(unsigned channel);
|
||||
__EXPORT int io_timer_is_channel_free(unsigned channel);
|
||||
__EXPORT int io_timer_free_channel(unsigned channel);
|
||||
__EXPORT int io_timer_get_channel_mode(unsigned channel);
|
||||
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
|
||||
__EXPORT extern void io_timer_trigger(void);
|
||||
|
||||
__END_DECLS
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_io_pins
|
||||
io_timer.c
|
||||
pwm_servo.c
|
||||
pwm_trigger.c
|
||||
input_capture.c
|
||||
)
|
||||
|
|
@ -0,0 +1,330 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_input_capture.c
|
||||
*
|
||||
* Servo driver supporting input capture connected to imxrt timer blocks.
|
||||
*
|
||||
* Works with any FLEXPWN that have input pins.
|
||||
*
|
||||
* Require an interrupt.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <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 <chip.h>
|
||||
#include "hardware/imxrt_flexpwm.h"
|
||||
|
||||
#define MAX_CHANNELS_PER_TIMER 2
|
||||
|
||||
#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET)
|
||||
|
||||
#define _REG(_addr) (*(volatile uint16_t *)(_addr))
|
||||
#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg))
|
||||
#define REG(_tmr, _sm, _reg) _REG16(io_timers[(_tmr)].base + ((_sm) * SM_SPACING), (_reg))
|
||||
|
||||
static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
static struct channel_handler_entry {
|
||||
capture_callback_t callback;
|
||||
void *context;
|
||||
} channel_handlers[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index,
|
||||
const timer_io_channels_t *chan,
|
||||
hrt_abstime isrs_time, uint16_t isrs_rcnt,
|
||||
uint16_t capture)
|
||||
{
|
||||
channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
|
||||
|
||||
if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
|
||||
channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
|
||||
}
|
||||
|
||||
channel_stats[chan_index].chan_in_edges_out++;
|
||||
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
|
||||
uint32_t overflow = 0;//_REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF;
|
||||
|
||||
if (overflow) {
|
||||
|
||||
/* Error we has a second edge before we cleared CCxR */
|
||||
|
||||
channel_stats[chan_index].overflows++;
|
||||
}
|
||||
|
||||
if (channel_handlers[chan_index].callback) {
|
||||
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index,
|
||||
channel_stats[chan_index].last_time,
|
||||
channel_stats[chan_index].last_edge, overflow);
|
||||
}
|
||||
}
|
||||
|
||||
static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
channel_handlers[channel].callback = callback;
|
||||
channel_handlers[channel].context = context;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
static void input_capture_unbind(unsigned channel)
|
||||
{
|
||||
input_capture_bind(channel, NULL, NULL);
|
||||
}
|
||||
|
||||
int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
|
||||
capture_callback_t callback, void *context)
|
||||
{
|
||||
if (edge > Both) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
if (edge == Disabled) {
|
||||
|
||||
io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel);
|
||||
input_capture_unbind(channel);
|
||||
|
||||
} else {
|
||||
|
||||
if (-EBUSY == io_timer_is_channel_free(channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
}
|
||||
|
||||
input_capture_bind(channel, callback, context);
|
||||
|
||||
rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context);
|
||||
|
||||
if (rv != 0) {
|
||||
return rv;
|
||||
}
|
||||
|
||||
rv = up_input_capture_set_filter(channel, filter);
|
||||
|
||||
if (rv == 0) {
|
||||
rv = up_input_capture_set_trigger(channel, edge);
|
||||
|
||||
if (rv == 0) {
|
||||
rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
int up_input_capture_set_filter(unsigned channel, capture_filter_t filter)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
rv = -ENXIO;
|
||||
|
||||
/* Any pins in capture mode */
|
||||
|
||||
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
|
||||
|
||||
rv = OK;
|
||||
|
||||
uint32_t timer = timer_io_channels[channel].timer_index;
|
||||
uint32_t offset = timer_io_channels[channel].val_offset == PWMA_VAL ? IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET :
|
||||
IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET;
|
||||
uint32_t rvalue = REG(timer, timer_io_channels[channel].sub_module, offset);
|
||||
rvalue &= SMC_EDGA0_BOTH;
|
||||
|
||||
switch (rvalue) {
|
||||
|
||||
case (SMC_EDGA0_RISING):
|
||||
*edge = Rising;
|
||||
break;
|
||||
|
||||
case (SMC_EDGA0_FALLING):
|
||||
*edge = Falling;
|
||||
break;
|
||||
|
||||
case (SMC_EDGA0_BOTH):
|
||||
*edge = Both;
|
||||
break;
|
||||
|
||||
default:
|
||||
rv = -EIO;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
rv = -ENXIO;
|
||||
|
||||
/* Any pins in capture mode */
|
||||
|
||||
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
|
||||
|
||||
uint16_t edge_bits = 0;
|
||||
|
||||
switch (edge) {
|
||||
case Disabled:
|
||||
break;
|
||||
|
||||
case Rising:
|
||||
edge_bits = SMC_EDGA0_RISING;
|
||||
break;
|
||||
|
||||
case Falling:
|
||||
edge_bits = SMC_EDGA0_FALLING;
|
||||
break;
|
||||
|
||||
case Both:
|
||||
edge_bits = SMC_EDGA0_BOTH;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;;
|
||||
}
|
||||
|
||||
uint32_t timer = timer_io_channels[channel].timer_index;
|
||||
uint32_t offset = timer_io_channels[channel].val_offset == PWMA_VAL ? IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET :
|
||||
IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET;
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
uint32_t rvalue = REG(timer, timer_io_channels[channel].sub_module, offset);
|
||||
rvalue &= ~SMC_EDGA0_BOTH;
|
||||
rvalue |= edge_bits;
|
||||
REG(timer, timer_io_channels[channel].sub_module, offset) = rvalue;
|
||||
px4_leave_critical_section(flags);
|
||||
rv = OK;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
rv = -ENXIO;
|
||||
|
||||
/* Any pins in capture mode */
|
||||
|
||||
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
*callback = channel_handlers[channel].callback;
|
||||
*context = channel_handlers[channel].context;
|
||||
px4_leave_critical_section(flags);
|
||||
rv = OK;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
|
||||
}
|
||||
|
||||
int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
rv = -ENXIO;
|
||||
|
||||
/* Any pins in capture mode */
|
||||
|
||||
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
|
||||
input_capture_bind(channel, callback, context);
|
||||
rv = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
*stats = channel_stats[channel];
|
||||
|
||||
if (clear) {
|
||||
memset(&channel_stats[channel], 0, sizeof(*stats));
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
|
@ -0,0 +1,733 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_io_timer.c
|
||||
*
|
||||
* Servo driver supporting PWM servos connected to imxrt FLEXPWM blocks.
|
||||
*/
|
||||
|
||||
#include <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 <chip.h>
|
||||
#include "hardware/imxrt_flexpwm.h"
|
||||
#include "imxrt_periphclks.h"
|
||||
|
||||
#if !defined(BOARD_PWM_FREQ)
|
||||
#define BOARD_PWM_FREQ 1000000
|
||||
#endif
|
||||
|
||||
#if !defined(BOARD_ONESHOT_FREQ)
|
||||
#define BOARD_ONESHOT_FREQ 8000000
|
||||
#endif
|
||||
|
||||
#define FLEXPWM_SRC_CLOCK_FREQ 16000000
|
||||
|
||||
#define MAX_CHANNELS_PER_TIMER 2
|
||||
|
||||
#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET)
|
||||
|
||||
#define _REG(_addr) (*(volatile uint16_t *)(_addr))
|
||||
#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg))
|
||||
#define REG(_tmr, _sm, _reg) _REG16(io_timers[(_tmr)].base + ((_sm) * SM_SPACING), (_reg))
|
||||
|
||||
|
||||
/* Timer register accessors */
|
||||
|
||||
#define rCNT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CNT_OFFSET) /* Counter Register */
|
||||
#define rINIT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */
|
||||
#define rCTRL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */
|
||||
#define rCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */
|
||||
#define rVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */
|
||||
#define rFRACVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL1_OFFSET) /* Fractional Value Register 1 */
|
||||
#define rVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */
|
||||
#define rFRACVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL2_OFFSET) /* Fractional Value Register 2 */
|
||||
#define rVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */
|
||||
#define rFRACVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL3_OFFSET) /* Fractional Value Register 3 */
|
||||
#define rVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */
|
||||
#define rFRACVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL4_OFFSET) /* Fractional Value Register 4 */
|
||||
#define rVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */
|
||||
#define rFRACVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRACVAL5_OFFSET) /* Fractional Value Register 5 */
|
||||
#define rVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */
|
||||
#define rFRCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0FRCTRL_OFFSET) /* Fractional Control Register */
|
||||
#define rOCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0OCTRL_OFFSET) /* Output Control Register */
|
||||
#define rSTS(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0STS_OFFSET) /* Status Register */
|
||||
#define rINTEN(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INTEN_OFFSET) /* Interrupt Enable Register */
|
||||
#define rDMAEN(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DMAEN_OFFSET) /* DMA Enable Register */
|
||||
#define rTCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0TCTRL_OFFSET) /* Output Trigger Control Register */
|
||||
#define rDISMAP0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */
|
||||
#define rDISMAP1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */
|
||||
#define rDTCNT0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DTCNT0_OFFSET) /* Deadtime Count Register 0 */
|
||||
#define rDTCNT1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DTCNT1_OFFSET) /* Deadtime Count Register 1 */
|
||||
#define rCAPTCTRLA(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLA_OFFSET) /* Capture Control A Register */
|
||||
#define rCAPTCOMPA(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPA_OFFSET) /* Capture Compare A Register */
|
||||
#define rCAPTCTRLB(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLB_OFFSET) /* Capture Control B Register */
|
||||
#define rCAPTCOMPB(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPB_OFFSET) /* Capture Compare B Register */
|
||||
#define rCAPTCTRLX(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCTRLX_OFFSET) /* Capture Control X Register */
|
||||
#define rCAPTCOMPX(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CAPTCOMPX_OFFSET) /* Capture Compare X Register */
|
||||
#define rCVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL0_OFFSET) /* Capture Value 0 Register */
|
||||
#define rCVAL0CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL0CYC_OFFSET) /* Capture Value 0 Cycle Register */
|
||||
#define rCVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL1_OFFSET) /* Capture Value 1 Register */
|
||||
#define rCVAL1CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL1CYC_OFFSET) /* Capture Value 1 Cycle Register */
|
||||
#define rCVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL2_OFFSET) /* Capture Value 2 Register */
|
||||
#define rCVAL2CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL2CYC_OFFSET) /* Capture Value 2 Cycle Register */
|
||||
#define rCVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL3_OFFSET) /* Capture Value 3 Register */
|
||||
#define rCVAL3CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL3CYC_OFFSET) /* Capture Value 3 Cycle Register */
|
||||
#define rCVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL4_OFFSET) /* Capture Value 4 Register */
|
||||
#define rCVAL4CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL4CYC_OFFSET) /* Capture Value 4 Cycle Register */
|
||||
#define rCVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL5_OFFSET) /* Capture Value 5 Register */
|
||||
#define rCVAL5CYC(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CVAL5CYC_OFFSET) /* Capture Value 5 Cycle Register */
|
||||
|
||||
#define rOUTEN(_tim) REG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */
|
||||
#define rMASK(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MASK_OFFSET) /* Mask Register */
|
||||
#define rSWCOUT(_tim) REG(_tim, 0, IMXRT_FLEXPWM_SWCOUT_OFFSET) /* Software Controlled Output Register */
|
||||
#define rDTSRCSEL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */
|
||||
#define rMCTRL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */
|
||||
#define rMCTRL2(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL2_OFFSET) /* Master Control 2 Register */
|
||||
#define rFCTRL0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL0_OFFSET) /* Fault Control Register */
|
||||
#define rFSTS0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */
|
||||
#define rFFILT0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */
|
||||
#define rFTST0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FTST0_OFFSET) /* Fault Test Register */
|
||||
#define rFCTRL20(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL20_OFFSET) /* Fault Control 2 Register */
|
||||
|
||||
|
||||
// NotUsed PWMOut PWMIn Capture OneShot Trigger
|
||||
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 };
|
||||
|
||||
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
|
||||
|
||||
static io_timer_allocation_t once = 0;
|
||||
|
||||
typedef struct channel_stat_t {
|
||||
uint32_t isr_cout;
|
||||
uint32_t overflows;
|
||||
} channel_stat_t;
|
||||
|
||||
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
static struct channel_handler_entry {
|
||||
channel_handler_t callback;
|
||||
void *context;
|
||||
} channel_handlers[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
|
||||
static int io_timer_handler(uint16_t timer_index)
|
||||
{
|
||||
// Not implemented yet
|
||||
UNUSED(io_timer_channel_stats);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_handler0(int irq, void *context, void *arg)
|
||||
{
|
||||
|
||||
return io_timer_handler(0);
|
||||
}
|
||||
|
||||
int io_timer_handler1(int irq, void *context, void *arg)
|
||||
{
|
||||
return io_timer_handler(1);
|
||||
|
||||
}
|
||||
|
||||
int io_timer_handler2(int irq, void *context, void *arg)
|
||||
{
|
||||
return io_timer_handler(2);
|
||||
|
||||
}
|
||||
|
||||
int io_timer_handler3(int irq, void *context, void *arg)
|
||||
{
|
||||
return io_timer_handler(3);
|
||||
|
||||
}
|
||||
|
||||
static inline int is_timer_uninitalized(unsigned timer)
|
||||
{
|
||||
int rv = 0;
|
||||
|
||||
if (once & 1 << timer) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static inline void set_timer_initalized(unsigned timer)
|
||||
{
|
||||
once |= 1 << timer;
|
||||
}
|
||||
|
||||
|
||||
static inline int channels_timer(unsigned channel)
|
||||
{
|
||||
return timer_io_channels[channel].timer_index;
|
||||
}
|
||||
|
||||
static uint32_t get_channel_mask(unsigned channel)
|
||||
{
|
||||
return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0;
|
||||
}
|
||||
|
||||
int io_timer_is_channel_free(unsigned channel)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_validate_channel_index(unsigned channel)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].val_offset != 0) {
|
||||
|
||||
/* test timer for validity */
|
||||
|
||||
if (io_timers[channels_timer(channel)].base != 0) {
|
||||
rv = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
|
||||
{
|
||||
if (mode < IOTimerChanModeSize) {
|
||||
return channel_allocations[mode];
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_get_channel_mode(unsigned channel)
|
||||
{
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
|
||||
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
|
||||
if (bit & channel_allocations[mode]) {
|
||||
return mode;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode,
|
||||
io_timer_channel_mode_t new_mode)
|
||||
{
|
||||
/* If caller mode is not based on current setting adjust it */
|
||||
|
||||
if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) {
|
||||
mode = IOTimerChanMode_NotUsed;
|
||||
}
|
||||
|
||||
/* Remove old set of channels from original */
|
||||
|
||||
channel_allocations[mode] &= ~channels;
|
||||
|
||||
/* Will this change ?*/
|
||||
|
||||
uint32_t before = channel_allocations[new_mode] & channels;
|
||||
|
||||
/* add in the new set */
|
||||
|
||||
channel_allocations[new_mode] |= channels;
|
||||
|
||||
/* Indicate a mode change */
|
||||
|
||||
return before ^ channels;
|
||||
}
|
||||
|
||||
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = io_timer_is_channel_free(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
|
||||
channel_allocations[mode] |= bit;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
static inline int free_channel_resource(unsigned channel)
|
||||
{
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[mode] &= ~bit;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
|
||||
}
|
||||
|
||||
return mode;
|
||||
}
|
||||
|
||||
int io_timer_free_channel(unsigned channel)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_set_enable(false, mode, 1 << channel);
|
||||
free_channel_resource(channel);
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (mode != IOTimerChanMode_NotUsed) {
|
||||
rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
rv = allocate_channel_resource(channel, mode);
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int timer_set_rate(unsigned channel, unsigned rate)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
|
||||
;
|
||||
rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
|
||||
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
|
||||
px4_leave_critical_section(flags);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void io_timer_set_oneshot_mode(unsigned channel)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
|
||||
rvalue &= ~SMCTRL_PRSC_MASK;
|
||||
rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
|
||||
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
|
||||
;
|
||||
rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue;
|
||||
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
}
|
||||
|
||||
static inline void io_timer_set_PWM_mode(unsigned channel)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
|
||||
rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
|
||||
rvalue |= SMCTRL_PRSC_DIV16;
|
||||
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
|
||||
;
|
||||
rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue;
|
||||
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
void io_timer_trigger(void)
|
||||
{
|
||||
int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);
|
||||
struct {
|
||||
uint32_t base;
|
||||
uint16_t triggers;
|
||||
} action_cache[MAX_IO_TIMERS] = {0};
|
||||
int actions = 0;
|
||||
|
||||
/* Pre-calculate the list of channels to Trigger */
|
||||
int mask;
|
||||
|
||||
for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
|
||||
action_cache[actions].base = io_timers[timer].base;
|
||||
|
||||
if (action_cache[actions].base) {
|
||||
for (uint32_t channel = io_timers[timer].first_channel_index;
|
||||
channel <= io_timers[timer].last_channel_index; channel++) {
|
||||
mask = get_channel_mask(channel);
|
||||
|
||||
if (oneshots & mask) {
|
||||
action_cache[actions].triggers |= timer_io_channels[channel].sub_module_bits;
|
||||
}
|
||||
}
|
||||
|
||||
actions++;
|
||||
}
|
||||
}
|
||||
|
||||
/* Now do them all with the shortest delay in between */
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
for (actions = 0; action_cache[actions].base != 0 && actions < MAX_IO_TIMERS; actions++) {
|
||||
_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= (action_cache[actions].triggers >> MCTRL_LDOK_SHIFT)
|
||||
<< MCTRL_CLDOK_SHIFT ;
|
||||
_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= action_cache[actions].triggers;
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
int io_timer_init_timer(unsigned timer)
|
||||
{
|
||||
/* Do this only once per timer */
|
||||
|
||||
int rv = is_timer_uninitalized(timer);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
set_timer_initalized(timer);
|
||||
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
|
||||
switch (io_timers[timer].base) {
|
||||
case IMXRT_FLEXPWM1_BASE:
|
||||
imxrt_clockall_pwm1();
|
||||
break;
|
||||
|
||||
case IMXRT_FLEXPWM2_BASE:
|
||||
imxrt_clockall_pwm2();
|
||||
break;
|
||||
|
||||
case IMXRT_FLEXPWM3_BASE:
|
||||
imxrt_clockall_pwm3();
|
||||
break;
|
||||
|
||||
case IMXRT_FLEXPWM4_BASE:
|
||||
imxrt_clockall_pwm4();
|
||||
break;
|
||||
}
|
||||
|
||||
for (uint32_t chan = io_timers[timer].first_channel_index;
|
||||
chan <= io_timers[timer].last_channel_index; chan++) {
|
||||
|
||||
/* Clear all Faults */
|
||||
rFSTS0(timer) = FSTS_FFLAG_MASK;
|
||||
|
||||
rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP;
|
||||
rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL;
|
||||
/* Edge aligned at 0 */
|
||||
rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
|
||||
rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
|
||||
rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
|
||||
rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
|
||||
rFFILT0(timer) &= ~FFILT_FILT_PER_MASK;
|
||||
rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
|
||||
rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
|
||||
rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
|
||||
: OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
|
||||
rDTSRCSEL(timer) = 0;
|
||||
rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
|
||||
rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
|
||||
io_timer_set_PWM_mode(chan);
|
||||
timer_set_rate(chan, 50);
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
int io_timer_set_rate(unsigned channel, unsigned rate)
|
||||
{
|
||||
int rv = EBUSY;
|
||||
|
||||
/* Get the channel bits that belong to the channel */
|
||||
|
||||
uint32_t channels = get_channel_mask(channel);
|
||||
|
||||
/* Check that all channels are either in PWM or Oneshot */
|
||||
|
||||
if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
|
||||
channel_allocations[IOTimerChanMode_OneShot] |
|
||||
channel_allocations[IOTimerChanMode_NotUsed])) ==
|
||||
channels) {
|
||||
|
||||
/* Change only a timer that is owned by pwm or one shot */
|
||||
|
||||
/* Request to use OneShot ?*/
|
||||
|
||||
if (rate == 0) {
|
||||
|
||||
/* Request to use OneShot
|
||||
*
|
||||
* We are here because ALL these channels were either PWM or Oneshot
|
||||
* Now they need to be Oneshot
|
||||
*/
|
||||
|
||||
/* Did the allocation change */
|
||||
if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
|
||||
io_timer_set_oneshot_mode(channel);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* Request to use PWM
|
||||
*
|
||||
* We are here because ALL these channels were either PWM or Oneshot
|
||||
* Now they need to be PWM
|
||||
*/
|
||||
|
||||
if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
|
||||
io_timer_set_PWM_mode(channel);
|
||||
}
|
||||
|
||||
timer_set_rate(channel, rate);
|
||||
}
|
||||
|
||||
rv = OK;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context)
|
||||
{
|
||||
uint32_t gpio = 0;
|
||||
|
||||
/* figure out the GPIO config first */
|
||||
|
||||
switch (mode) {
|
||||
|
||||
case IOTimerChanMode_OneShot:
|
||||
case IOTimerChanMode_PWMOut:
|
||||
case IOTimerChanMode_Trigger:
|
||||
gpio = timer_io_channels[channel].gpio_out;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_PWMIn:
|
||||
case IOTimerChanMode_Capture:
|
||||
return -EINVAL;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_NotUsed:
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int rv = allocate_channel(channel, mode);
|
||||
|
||||
/* Valid channel should now be reserved in new mode */
|
||||
|
||||
if (rv >= 0) {
|
||||
|
||||
unsigned timer = channels_timer(channel);
|
||||
|
||||
/* Blindly try to initialize the timer - it will only do it once */
|
||||
|
||||
io_timer_init_timer(timer);
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
/* Set up IO */
|
||||
if (gpio) {
|
||||
px4_arch_configgpio(gpio);
|
||||
}
|
||||
|
||||
/* configure the channel */
|
||||
|
||||
REG(timer, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) |= MCTRL_RUN(1 << timer_io_channels[channel].sub_module);
|
||||
|
||||
channel_handlers[channel].callback = channel_handler;
|
||||
channel_handlers[channel].context = context;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
|
||||
{
|
||||
switch (mode) {
|
||||
case IOTimerChanMode_NotUsed:
|
||||
case IOTimerChanMode_PWMOut:
|
||||
case IOTimerChanMode_OneShot:
|
||||
case IOTimerChanMode_Trigger:
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_PWMIn:
|
||||
case IOTimerChanMode_Capture:
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* Was the request for all channels in this mode ?*/
|
||||
|
||||
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
|
||||
|
||||
/* Yes - we provide them */
|
||||
|
||||
masks = channel_allocations[mode];
|
||||
|
||||
} else {
|
||||
|
||||
/* No - caller provided mask */
|
||||
|
||||
/* Only allow the channels in that mode to be affected */
|
||||
|
||||
masks &= channel_allocations[mode];
|
||||
|
||||
}
|
||||
|
||||
struct {
|
||||
uint32_t sm_ens;
|
||||
uint32_t base;
|
||||
uint32_t io_index;
|
||||
uint32_t gpios[MAX_TIMER_IO_CHANNELS];
|
||||
} action_cache[MAX_IO_TIMERS];
|
||||
|
||||
memset(action_cache, 0, sizeof(action_cache));
|
||||
|
||||
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
|
||||
if (masks & (1 << chan_index)) {
|
||||
masks &= ~(1 << chan_index);
|
||||
|
||||
if (io_timer_validate_channel_index(chan_index) == 0) {
|
||||
uint32_t timer_index = channels_timer(chan_index);
|
||||
action_cache[timer_index].base = io_timers[timer_index].base;
|
||||
action_cache[timer_index].sm_ens |= MCTRL_RUN(1 << timer_io_channels[chan_index].sub_module) |
|
||||
timer_io_channels[chan_index].sub_module_bits;
|
||||
action_cache[timer_index].gpios[action_cache[timer_index].io_index++] = timer_io_channels[chan_index].gpio_out;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
|
||||
if (action_cache[actions].base != 0) {
|
||||
for (unsigned int index = 0; index < action_cache[actions].io_index; index++) {
|
||||
if (action_cache[actions].gpios[index]) {
|
||||
px4_arch_configgpio(action_cache[actions].gpios[index]);
|
||||
}
|
||||
|
||||
_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) = action_cache[actions].sm_ens;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_set_ccr(unsigned channel, uint16_t value)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if ((mode != IOTimerChanMode_PWMOut) &&
|
||||
(mode != IOTimerChanMode_OneShot) &&
|
||||
(mode != IOTimerChanMode_Trigger)) {
|
||||
|
||||
rv = -EIO;
|
||||
|
||||
} else {
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
|
||||
;
|
||||
REG(channels_timer(channel), timer_io_channels[channel].sub_module, timer_io_channels[channel].val_offset) = value - 1;
|
||||
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
uint16_t io_channel_get_ccr(unsigned channel)
|
||||
{
|
||||
uint16_t value = 0;
|
||||
|
||||
if (io_timer_validate_channel_index(channel) == 0) {
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if ((mode == IOTimerChanMode_PWMOut) ||
|
||||
(mode == IOTimerChanMode_OneShot) ||
|
||||
(mode == IOTimerChanMode_Trigger)) {
|
||||
value = REG(channels_timer(channel), timer_io_channels[channel].sub_module, timer_io_channels[channel].val_offset) + 1;
|
||||
}
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
// The rt has 1:1 group to channel
|
||||
uint32_t io_timer_get_group(unsigned group)
|
||||
{
|
||||
return get_channel_mask(group);
|
||||
}
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_pwm_servo.c
|
||||
*
|
||||
* Servo driver supporting PWM servos connected to FLexPWM timer blocks.
|
||||
* N.B. Groups:channels have a 1:1 correspondence on FlexPWM
|
||||
*
|
||||
*/
|
||||
|
||||
#include <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 <chip.h>
|
||||
|
||||
int up_pwm_servo_set(unsigned channel, servo_position_t value)
|
||||
{
|
||||
return io_timer_set_ccr(channel, value);
|
||||
}
|
||||
|
||||
servo_position_t up_pwm_servo_get(unsigned channel)
|
||||
{
|
||||
return io_channel_get_ccr(channel);
|
||||
}
|
||||
|
||||
int up_pwm_servo_init(uint32_t channel_mask)
|
||||
{
|
||||
/* Init channels */
|
||||
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
|
||||
|
||||
// First free the current set of PWMs
|
||||
|
||||
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (current & (1 << channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
current &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
// Now allocate the new set
|
||||
|
||||
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (channel_mask & (1 << channel)) {
|
||||
|
||||
// First free any that were not PWM mode before
|
||||
|
||||
if (-EBUSY == io_timer_is_channel_free(channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
}
|
||||
|
||||
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
|
||||
channel_mask &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void up_pwm_servo_deinit(void)
|
||||
{
|
||||
/* disable the timers */
|
||||
up_pwm_servo_arm(false);
|
||||
}
|
||||
|
||||
int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) < 0) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* Allow a rate of 0 to enter oneshot mode */
|
||||
|
||||
if (rate != 0) {
|
||||
|
||||
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
|
||||
|
||||
if (rate < 1) {
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
if (rate > 10000) {
|
||||
return -ERANGE;
|
||||
}
|
||||
}
|
||||
|
||||
return io_timer_set_rate(channel, rate);
|
||||
}
|
||||
|
||||
void up_pwm_update(void)
|
||||
{
|
||||
io_timer_trigger();
|
||||
}
|
||||
|
||||
int up_pwm_servo_set_rate(unsigned rate)
|
||||
{
|
||||
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; i++) {
|
||||
up_pwm_servo_set_rate_group_update(i, rate);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t up_pwm_servo_get_rate_group(unsigned group)
|
||||
{
|
||||
return io_timer_get_group(group);
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_arm(bool armed)
|
||||
{
|
||||
io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
|
||||
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
|
||||
}
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_pwm_trigger.c
|
||||
*
|
||||
*/
|
||||
|
||||
#include <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 */
|
||||
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (channel_mask & (1 << channel)) {
|
||||
|
||||
// First free any that were not trigger mode before
|
||||
if (-EBUSY == io_timer_is_channel_free(channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
}
|
||||
|
||||
io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
|
||||
channel_mask &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
/* Enable the timers */
|
||||
up_pwm_trigger_arm(true);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void up_pwm_trigger_deinit()
|
||||
{
|
||||
/* Disable the timers */
|
||||
up_pwm_trigger_arm(false);
|
||||
|
||||
/* Deinit channels */
|
||||
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_Trigger);
|
||||
|
||||
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (current & (1 << channel)) {
|
||||
|
||||
io_timer_channel_init(channel, IOTimerChanMode_NotUsed, NULL, NULL);
|
||||
current &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_trigger_arm(bool armed)
|
||||
{
|
||||
io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS);
|
||||
}
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,354 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name Airmind nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_led_pwm.cpp
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <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 <chip.h>
|
||||
#include "hardware/imxrt_tmr.h"
|
||||
|
||||
int led_pwm_servo_set(unsigned channel, uint8_t cvalue)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
int led_pwm_servo_init(void)
|
||||
{
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
#if 0 && defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM)
|
||||
|
||||
#define FTM_SRC_CLOCK_FREQ 16000000
|
||||
#define LED_PWM_FREQ 1000000
|
||||
|
||||
#if (BOARD_LED_PWM_RATE)
|
||||
# define LED_PWM_RATE BOARD_LED_PWM_RATE
|
||||
#else
|
||||
# define LED_PWM_RATE 50
|
||||
#endif
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
|
||||
#define REG(_tmr, _reg) _REG32(led_pwm_timers[_tmr].base, _reg)
|
||||
|
||||
|
||||
/* Timer register accessors */
|
||||
|
||||
#define rSC(_tmr) REG(_tmr,KINETIS_FTM_SC_OFFSET)
|
||||
#define rCNT(_tmr) REG(_tmr,KINETIS_FTM_CNT_OFFSET)
|
||||
#define rMOD(_tmr) REG(_tmr,KINETIS_FTM_MOD_OFFSET)
|
||||
#define rC0SC(_tmr) REG(_tmr,KINETIS_FTM_C0SC_OFFSET)
|
||||
#define rC0V(_tmr) REG(_tmr,KINETIS_FTM_C0V_OFFSET)
|
||||
#define rC1SC(_tmr) REG(_tmr,KINETIS_FTM_C1SC_OFFSET)
|
||||
#define rC1V(_tmr) REG(_tmr,KINETIS_FTM_C1V_OFFSET)
|
||||
#define rC2SC(_tmr) REG(_tmr,KINETIS_FTM_C2SC_OFFSET)
|
||||
#define rC2V(_tmr) REG(_tmr,KINETIS_FTM_C2V_OFFSET)
|
||||
#define rC3SC(_tmr) REG(_tmr,KINETIS_FTM_C3SC_OFFSET)
|
||||
#define rC3V(_tmr) REG(_tmr,KINETIS_FTM_C3V_OFFSET)
|
||||
#define rC4SC(_tmr) REG(_tmr,KINETIS_FTM_C4SC_OFFSET)
|
||||
#define rC4V(_tmr) REG(_tmr,KINETIS_FTM_C4V_OFFSET)
|
||||
#define rC5SC(_tmr) REG(_tmr,KINETIS_FTM_C5SC_OFFSET)
|
||||
#define rC5V(_tmr) REG(_tmr,KINETIS_FTM_C5V_OFFSET)
|
||||
#define rC6SC(_tmr) REG(_tmr,KINETIS_FTM_C6SC_OFFSET)
|
||||
#define rC6V(_tmr) REG(_tmr,KINETIS_FTM_C6V_OFFSET)
|
||||
#define rC7SC(_tmr) REG(_tmr,KINETIS_FTM_C7SC_OFFSET)
|
||||
#define rC7V(_tmr) REG(_tmr,KINETIS_FTM_C7V_OFFSET)
|
||||
|
||||
#define rCNTIN(_tmr) REG(_tmr,KINETIS_FTM_CNTIN_OFFSET)
|
||||
#define rSTATUS(_tmr) REG(_tmr,KINETIS_FTM_STATUS_OFFSET)
|
||||
#define rMODE(_tmr) REG(_tmr,KINETIS_FTM_MODE_OFFSET)
|
||||
#define rSYNC(_tmr) REG(_tmr,KINETIS_FTM_SYNC_OFFSET)
|
||||
#define rOUTINIT(_tmr) REG(_tmr,KINETIS_FTM_OUTINIT_OFFSET)
|
||||
#define rOUTMASK(_tmr) REG(_tmr,KINETIS_FTM_OUTMASK_OFFSET)
|
||||
#define rCOMBINE(_tmr) REG(_tmr,KINETIS_FTM_COMBINE_OFFSET)
|
||||
#define rDEADTIME(_tmr) REG(_tmr,KINETIS_FTM_DEADTIME_OFFSET)
|
||||
#define rEXTTRIG(_tmr) REG(_tmr,KINETIS_FTM_EXTTRIG_OFFSET)
|
||||
#define rPOL(_tmr) REG(_tmr,KINETIS_FTM_POL_OFFSET)
|
||||
#define rFMS(_tmr) REG(_tmr,KINETIS_FTM_FMS_OFFSET)
|
||||
#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET)
|
||||
#define rFLTCTRL(_tmr) REG(_tmr,KINETIS_FTM_FLTCTRL_OFFSET)
|
||||
#define rQDCTRL(_tmr) REG(_tmr,KINETIS_FTM_QDCTRL_OFFSET)
|
||||
#define rCONF(_tmr) REG(_tmr,KINETIS_FTM_CONF_OFFSET)
|
||||
#define rFLTPOL(_tmr) REG(_tmr,KINETIS_FTM_FLTPOL_OFFSET)
|
||||
#define rSYNCONF(_tmr) REG(_tmr,KINETIS_FTM_SYNCONF_OFFSET)
|
||||
#define rINVCTRL(_tmr) REG(_tmr,KINETIS_FTM_INVCTRL_OFFSET)
|
||||
#define rSWOCTRL(_tmr) REG(_tmr,KINETIS_FTM_SWOCTRL_OFFSET)
|
||||
#define rPWMLOAD(_tmr) REG(_tmr,KINETIS_FTM_PWMLOAD_OFFSET)
|
||||
|
||||
#define CnSC_RESET (FTM_CSC_CHF|FTM_CSC_CHIE|FTM_CSC_MSB|FTM_CSC_MSA|FTM_CSC_ELSB|FTM_CSC_ELSA|FTM_CSC_DMA)
|
||||
#define CnSC_CAPTURE_INIT (FTM_CSC_CHIE|FTM_CSC_ELSB|FTM_CSC_ELSA) // Both
|
||||
|
||||
#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW)
|
||||
#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSA)
|
||||
#else
|
||||
#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSB)
|
||||
#endif
|
||||
|
||||
#define FTM_SYNC (FTM_SYNC_SWSYNC)
|
||||
|
||||
static void led_pwm_timer_init(unsigned timer);
|
||||
static void led_pwm_timer_set_rate(unsigned timer, unsigned rate);
|
||||
static void led_pwm_channel_init(unsigned channel);
|
||||
|
||||
int led_pwm_servo_set(unsigned channel, uint8_t value);
|
||||
unsigned led_pwm_servo_get(unsigned channel);
|
||||
int led_pwm_servo_init(void);
|
||||
void led_pwm_servo_deinit(void);
|
||||
void led_pwm_servo_arm(bool armed);
|
||||
unsigned led_pwm_timer_get_period(unsigned timer);
|
||||
|
||||
static void led_pwm_timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
uint32_t save = rSC(timer);
|
||||
rSC(timer) = save & ~(FTM_SC_CLKS_MASK);
|
||||
|
||||
/* configure the timer to update at the desired rate */
|
||||
rMOD(timer) = (LED_PWM_FREQ / rate) - 1;
|
||||
rSC(timer) = save;
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
static inline uint32_t div2psc(int div)
|
||||
{
|
||||
return 31 - __builtin_clz(div);
|
||||
}
|
||||
|
||||
static inline void led_pwm_timer_set_PWM_mode(unsigned timer)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK);
|
||||
rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / LED_PWM_FREQ));
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
led_pwm_timer_init(unsigned timer)
|
||||
{
|
||||
/* valid Timer */
|
||||
|
||||
if (led_pwm_timers[timer].base != 0) {
|
||||
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
|
||||
uint32_t regval = _REG(led_pwm_timers[timer].clock_register);
|
||||
regval |= led_pwm_timers[timer].clock_bit;
|
||||
_REG(led_pwm_timers[timer].clock_register) = regval;
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
rSC(timer) = FTM_SC_CLKS_NONE;
|
||||
rCNT(timer) = 0;
|
||||
|
||||
rMODE(timer) = 0;
|
||||
rSYNCONF(timer) = (FTM_SYNCONF_SYNCMODE | FTM_SYNCONF_SWWRBUF | FTM_SYNCONF_SWRSTCNT);
|
||||
|
||||
/* Set to run in debug mode */
|
||||
|
||||
rCONF(timer) |= FTM_CONF_BDMMODE_MASK;
|
||||
|
||||
/* enable the timer */
|
||||
|
||||
led_pwm_timer_set_PWM_mode(timer);
|
||||
|
||||
/*
|
||||
* Note we do the Standard PWM Out init here
|
||||
* default to updating at LED_PWM_RATE
|
||||
*/
|
||||
|
||||
led_pwm_timer_set_rate(timer, LED_PWM_RATE);
|
||||
}
|
||||
}
|
||||
unsigned
|
||||
led_pwm_timer_get_period(unsigned timer)
|
||||
{
|
||||
// MOD is a 16 bit reg
|
||||
unsigned mod = rMOD(timer);
|
||||
|
||||
if (mod == 0) {
|
||||
return 1 << 16;
|
||||
}
|
||||
|
||||
return (uint16_t)(mod + 1);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
led_pwm_channel_init(unsigned channel)
|
||||
{
|
||||
/* Only initialize used channels */
|
||||
|
||||
if (led_pwm_channels[channel].timer_channel) {
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
/* configure the GPIO first */
|
||||
|
||||
px4_arch_configgpio(led_pwm_channels[channel].gpio_out);
|
||||
|
||||
/* configure the channel */
|
||||
|
||||
uint32_t chan = led_pwm_channels[channel].timer_channel - 1;
|
||||
|
||||
uint16_t rvalue = REG(timer, KINETIS_FTM_CSC_OFFSET(chan));
|
||||
rvalue &= ~CnSC_RESET;
|
||||
rvalue |= CnSC_PWMOUT_INIT;
|
||||
REG(timer, KINETIS_FTM_CSC_OFFSET(chan)) = rvalue;
|
||||
REG(timer, KINETIS_FTM_CV_OFFSET(0)) = 0;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
led_pwm_servo_set(unsigned channel, uint8_t cvalue)
|
||||
{
|
||||
if (channel >= arraySize(led_pwm_channels)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((led_pwm_timers[timer].base == 0) ||
|
||||
(led_pwm_channels[channel].gpio_out == 0)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned period = led_pwm_timer_get_period(timer);
|
||||
|
||||
unsigned value = (unsigned)cvalue * period / 255;
|
||||
|
||||
/* configure the channel */
|
||||
if (value > 0) {
|
||||
value--;
|
||||
}
|
||||
|
||||
REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1)) = value;
|
||||
|
||||
return 0;
|
||||
}
|
||||
unsigned
|
||||
led_pwm_servo_get(unsigned channel)
|
||||
{
|
||||
if (channel >= 3) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned timer = led_pwm_channels[channel].timer_index;
|
||||
servo_position_t value = 0;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((led_pwm_timers[timer].base == 0) ||
|
||||
(led_pwm_channels[channel].timer_channel == 0)) {
|
||||
return value;
|
||||
}
|
||||
|
||||
value = REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1));
|
||||
unsigned period = led_pwm_timer_get_period(timer);
|
||||
return ((value + 1) * 255 / period);
|
||||
}
|
||||
int
|
||||
led_pwm_servo_init(void)
|
||||
{
|
||||
/* do basic timer initialisation first */
|
||||
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
|
||||
led_pwm_timer_init(i);
|
||||
}
|
||||
|
||||
/* now init channels */
|
||||
for (unsigned i = 0; i < arraySize(led_pwm_channels); i++) {
|
||||
led_pwm_channel_init(i);
|
||||
}
|
||||
|
||||
led_pwm_servo_arm(true);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
led_pwm_servo_deinit(void)
|
||||
{
|
||||
/* disable the timers */
|
||||
led_pwm_servo_arm(false);
|
||||
}
|
||||
|
||||
void
|
||||
led_pwm_servo_arm(bool armed)
|
||||
{
|
||||
/* iterate timers and arm/disarm appropriately */
|
||||
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
|
||||
if (led_pwm_timers[i].base != 0) {
|
||||
if (armed) {
|
||||
/* force an update to preload all registers */
|
||||
led_pwm_timer_set_PWM_mode(i);
|
||||
|
||||
} else {
|
||||
/* disable and configure the timer */
|
||||
|
||||
rSC(i) = FTM_SC_CLKS_NONE;
|
||||
rCNT(i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // BOARD_HAS_LED_PWM || BOARD_HAS_UI_LED_PWM
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ToneAlarmInterface.cpp
|
||||
*/
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "hardware/imxrt_gpt.h"
|
||||
#include "imxrt_periphclks.h"
|
||||
|
||||
#define CAT3_(A, B, C) A##B##C
|
||||
#define CAT3(A, B, C) CAT3_(A, B, C)
|
||||
|
||||
#define CAT2_(A, B) A##B
|
||||
#define CAT2(A, B) CAT2_(A, B)
|
||||
|
||||
/* Check that tone alarm and HRT timers are different */
|
||||
#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
|
||||
# if TONE_ALARM_TIMER == HRT_TIMER
|
||||
# error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Period of the free-running counter, in microseconds.
|
||||
*/
|
||||
#define TONE_ALARM_COUNTER_PERIOD 4294967296
|
||||
|
||||
/* Tone Alarm configuration */
|
||||
|
||||
#define TONE_ALARM_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */
|
||||
#define TONE_ALARM_TIMER_BASE CAT3(IMXRT_GPT, TONE_ALARM_TIMER,_BASE) /* The Base address of the GPT */
|
||||
#define TONE_ALARM_TIMER_VECTOR CAT(IMXRT_IRQ_GPT, TONE_ALARM_TIMER) /* The GPT Interrupt vector */
|
||||
|
||||
#if TONE_ALARM_TIMER == 1
|
||||
# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif TONE_ALARM_TIMER == 2
|
||||
# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */
|
||||
#endif
|
||||
|
||||
#if TONE_ALARM_TIMER == 1 && defined(CONFIG_IMXRT_GPT1)
|
||||
# error must not set CONFIG_IMXRT_GPT1=y and TONE_ALARM_TIMER=1
|
||||
#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_IMXRT_GPT2)
|
||||
# error must not set CONFIG_IMXRT_GPT2=y and TONE_ALARM_TIMER=2
|
||||
#endif
|
||||
|
||||
|
||||
# define TONE_ALARM_TIMER_FREQ 1000000
|
||||
|
||||
/*
|
||||
* Tone Alarm clock must be a multiple of 1MHz greater than 1MHz
|
||||
*/
|
||||
#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_TIMER_FREQ) != 0
|
||||
# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz
|
||||
#endif
|
||||
#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_TIMER_FREQ
|
||||
# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz
|
||||
#endif
|
||||
|
||||
#if (TONE_ALARM_TIMER_CHANNEL > 1) || (TONE_ALARM_TIMER_CHANNEL > 3)
|
||||
# error TONE_ALARM_CHANNEL must be a value between 1 and 3
|
||||
#endif
|
||||
|
||||
|
||||
/* Register accessors */
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* Timer register accessors */
|
||||
|
||||
#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg))
|
||||
|
||||
#define rCR REG(IMXRT_GPT_CR_OFFSET)
|
||||
#define rPR REG(IMXRT_GPT_PR_OFFSET)
|
||||
#define rSR REG(IMXRT_GPT_SR_OFFSET)
|
||||
#define rIR REG(IMXRT_GPT_IR_OFFSET)
|
||||
#define rOCR1 REG(IMXRT_GPT_OCR1_OFFSET)
|
||||
#define rOCR2 REG(IMXRT_GPT_OCR2_OFFSET)
|
||||
#define rOCR3 REG(IMXRT_GPT_OCR3_OFFSET)
|
||||
#define rICR1 REG(IMXRT_GPT_ICR1_OFFSET)
|
||||
#define rICR2 REG(IMXRT_GPT_ICR2_OFFSET)
|
||||
#define rCNT REG(IMXRT_GPT_CNT_OFFSET)
|
||||
|
||||
/*
|
||||
* Specific registers and bits used by Tone Alarm sub-functions
|
||||
*/
|
||||
|
||||
#define rOCR CAT2(rOCR, TONE_ALARM_CHANNEL) /* GPT Output Compare Register used by HRT */
|
||||
#define rSTATUS CAT2(GPT_SR_OF, TONE_ALARM_CHANNEL) /* OF Output Compare Flag */
|
||||
#define CR_OM CAT3(GPT_CR_OM, TONE_ALARM_CHANNEL,_TOGGLE) /* Output Compare mode */
|
||||
|
||||
|
||||
#define CBRK_BUZZER_KEY 782097
|
||||
|
||||
namespace ToneAlarmInterface
|
||||
{
|
||||
|
||||
void init()
|
||||
{
|
||||
#if defined(TONE_ALARM_TIMER)
|
||||
/* configure the GPIO to the idle state */
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
|
||||
/* Enable the Module clock */
|
||||
|
||||
TONE_ALARM_CLOCK_ALL();
|
||||
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
rCR = GPT_CR_OM1_DIS | GPT_CR_OM2_DIS | GPT_CR_OM3_DIS |
|
||||
CR_OM | GPT_CR_FRR | GPT_CR_CLKSRC_IPG;
|
||||
|
||||
/* CLKSRC field is divided by [PRESCALER + 1] */
|
||||
|
||||
rPR = (TONE_ALARM_TIMER_CLOCK / TONE_ALARM_TIMER_FREQ) - 1;
|
||||
|
||||
/* enable the timer and output toggle */
|
||||
|
||||
rCR |= GPT_CR_EN;
|
||||
#endif /* TONE_ALARM_TIMER */
|
||||
}
|
||||
|
||||
void start_note(unsigned frequency)
|
||||
{
|
||||
#if defined(TONE_ALARM_TIMER)
|
||||
float period = 0.5f / frequency;
|
||||
|
||||
// and the divisor, rounded to the nearest integer
|
||||
unsigned divisor = (period * TONE_ALARM_TIMER_FREQ) + 0.5f;
|
||||
|
||||
rCR &= ~GPT_CR_EN;
|
||||
rOCR = divisor; // load new toggle period
|
||||
rCR |= GPT_CR_EN;
|
||||
|
||||
// configure the GPIO to enable timer output
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM);
|
||||
#endif /* TONE_ALARM_TIMER */
|
||||
}
|
||||
|
||||
void stop_note()
|
||||
{
|
||||
#if defined(TONE_ALARM_TIMER)
|
||||
/* stop the current note */
|
||||
|
||||
rCR &= ~GPT_CR_EN;
|
||||
|
||||
/*
|
||||
* Make sure the GPIO is not driving the speaker.
|
||||
*/
|
||||
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
|
||||
#endif /* TONE_ALARM_TIMER */
|
||||
}
|
||||
|
||||
} /* namespace ToneAlarmInterface */
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,170 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 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 imxrt based Board identity API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <up_arch.h>
|
||||
#include <hardware/imxrt_ocotp.h>
|
||||
|
||||
#define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4}
|
||||
#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24))
|
||||
|
||||
|
||||
static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID;
|
||||
|
||||
/* A type suitable for holding the reordering array for the byte format of the UUID
|
||||
*/
|
||||
|
||||
typedef const uint8_t uuid_uint8_reorder_t[PX4_CPU_UUID_BYTE_LENGTH];
|
||||
|
||||
void board_get_uuid(uuid_byte_t uuid_bytes)
|
||||
{
|
||||
uuid_uint8_reorder_t reorder = CPU_UUID_BYTE_FORMAT_ORDER;
|
||||
|
||||
union {
|
||||
uuid_byte_t b;
|
||||
uuid_uint32_t w;
|
||||
} id;
|
||||
|
||||
/* Copy the serial from the OCOTP */
|
||||
|
||||
board_get_uuid32(id.w);
|
||||
|
||||
/* swap endianess */
|
||||
|
||||
for (int i = 0; i < PX4_CPU_UUID_BYTE_LENGTH; i++) {
|
||||
uuid_bytes[i] = id.b[reorder[i]];
|
||||
}
|
||||
}
|
||||
|
||||
void board_get_uuid32(uuid_uint32_t uuid_words)
|
||||
{
|
||||
/* IMXRT_OCOTP_CFG1:0x420[10:0], IMXRT_OCOTP_CFG0:0x410[31:0] LOT_NO_ENC[42:0](SJC_CHALL/UNIQUE_ID[42:0])
|
||||
* 43 bits FSL-wide unique,encoded LOT ID STD II/SJC CHALLENGE/ Unique ID
|
||||
* 0x420[15:11] WAFER_NO[4:0]( SJC_CHALL[47:43] /UNIQUE_ID[47:43])
|
||||
* 5 bits The wafer number of the wafer on which the device was fabricated/SJC CHALLENGE/ Unique ID
|
||||
* 0x420[23:16] DIE-YCORDINATE[7:0]( SJC_CHALL[55:48] /UNIQUE_ID[55:48])
|
||||
* 8 bits The Y-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID
|
||||
* 0x420[31:24] DIE-XCORDINATE[7:0]( SJC_CHALL[63:56] /UNIQUE_ID[63:56] )
|
||||
* 8 bits The X-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID
|
||||
*
|
||||
* word [0] word[1]
|
||||
* SJC_CHALL[63:32] [31:00]
|
||||
*/
|
||||
|
||||
uuid_words[0] = getreg32(IMXRT_OCOTP_CFG1);
|
||||
uuid_words[1] = getreg32(IMXRT_OCOTP_CFG0);
|
||||
}
|
||||
|
||||
int board_get_uuid32_formated(char *format_buffer, int size,
|
||||
const char *format,
|
||||
const char *seperator)
|
||||
{
|
||||
uuid_uint32_t uuid;
|
||||
board_get_uuid32(uuid);
|
||||
|
||||
int offset = 0;
|
||||
int sep_size = seperator ? strlen(seperator) : 0;
|
||||
|
||||
for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]);
|
||||
|
||||
if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
|
||||
strcat(&format_buffer[offset], seperator);
|
||||
offset += sep_size;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int board_get_mfguid(mfguid_t mfgid)
|
||||
{
|
||||
board_get_uuid(* (uuid_byte_t *) mfgid);
|
||||
return PX4_CPU_MFGUID_BYTE_LENGTH;
|
||||
}
|
||||
|
||||
int board_get_mfguid_formated(char *format_buffer, int size)
|
||||
{
|
||||
mfguid_t mfguid;
|
||||
|
||||
board_get_mfguid(mfguid);
|
||||
int offset = 0;
|
||||
|
||||
for (unsigned int i = 0; i < PX4_CPU_MFGUID_BYTE_LENGTH; i++) {
|
||||
offset += snprintf(&format_buffer[offset], size - offset, "%02x", mfguid[i]);
|
||||
}
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
||||
int board_get_px4_guid(px4_guid_t px4_guid)
|
||||
{
|
||||
uint8_t *pb = (uint8_t *) &px4_guid[0];
|
||||
*pb++ = (soc_arch_id >> 8) & 0xff;
|
||||
*pb++ = (soc_arch_id & 0xff);
|
||||
|
||||
for (unsigned i = 0; i < PX4_GUID_BYTE_LENGTH - (sizeof(soc_arch_id) + PX4_CPU_UUID_BYTE_LENGTH); i++) {
|
||||
*pb++ = 0;
|
||||
}
|
||||
|
||||
board_get_uuid(pb);
|
||||
return PX4_GUID_BYTE_LENGTH;
|
||||
}
|
||||
|
||||
int board_get_px4_guid_formated(char *format_buffer, int size)
|
||||
{
|
||||
px4_guid_t px4_guid;
|
||||
board_get_px4_guid(px4_guid);
|
||||
int offset = 0;
|
||||
|
||||
/* size should be 2 per byte + 1 for termination
|
||||
* So it needs to be odd
|
||||
*/
|
||||
size = size & 1 ? size : size - 1;
|
||||
|
||||
/* Discard from MSD */
|
||||
for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) {
|
||||
offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]);
|
||||
}
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018, 2019 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_mcu_version.c
|
||||
* Implementation of imxrt based SoC version API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include <chip.h>
|
||||
#include <hardware/imxrt_usb_analog.h>
|
||||
#include "up_arch.h"
|
||||
|
||||
#define DIGPROG_MINOR_SHIFT 0
|
||||
#define DIGPROG_MINOR_MASK (0xff << DIGPROG_MINOR_SHIFT)
|
||||
#define DIGPROG_MINOR(info) (((info) & DIGPROG_MINOR_MASK) >> DIGPROG_MINOR_SHIFT)
|
||||
#define DIGPROG_MAJOR_LOWER_SHIFT 8
|
||||
#define DIGPROG_MAJOR_LOWER_MASK (0xff << DIGPROG_MAJOR_LOWER_SHIFT)
|
||||
#define DIGPROG_MAJOR_LOWER(info) (((info) & DIGPROG_MAJOR_LOWER_MASK) >> DIGPROG_MAJOR_LOWER_SHIFT)
|
||||
#define DIGPROG_MAJOR_UPPER_SHIFT 16
|
||||
#define DIGPROG_MAJOR_UPPER_MASK (0xff << DIGPROG_MAJOR_UPPER_SHIFT)
|
||||
#define DIGPROG_MAJOR_UPPER(info) (((info) & DIGPROG_MAJOR_UPPER_MASK) >> DIGPROG_MAJOR_UPPER_SHIFT)
|
||||
// 876543210
|
||||
#define CHIP_TAG "i.MX RT10?2 r?.?"
|
||||
#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1
|
||||
|
||||
int board_mcu_version(char *rev, const char **revstr, const char **errata)
|
||||
{
|
||||
uint32_t info = getreg32(IMXRT_USB_ANALOG_DIGPROG);
|
||||
static char chip[sizeof(CHIP_TAG)] = CHIP_TAG;
|
||||
|
||||
chip[CHIP_TAG_LEN - 1] = '0' + DIGPROG_MINOR(info);
|
||||
chip[CHIP_TAG_LEN - 3] = '1' + DIGPROG_MAJOR_LOWER(info);
|
||||
chip[CHIP_TAG_LEN - 7] = DIGPROG_MAJOR_UPPER(info) == 0x6a ? '5' : '6';
|
||||
*revstr = chip;
|
||||
*rev = '0' + DIGPROG_MINOR(info);
|
||||
|
||||
if (errata) {
|
||||
*errata = NULL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
|
||||
add_subdirectory(../imxrt/adc adc)
|
||||
add_subdirectory(../imxrt/board_critmon board_critmon)
|
||||
add_subdirectory(../imxrt/board_hw_info board_hw_info)
|
||||
add_subdirectory(../imxrt/board_reset board_reset)
|
||||
#add_subdirectory(../imxrt/dshot dshot)
|
||||
add_subdirectory(../imxrt/hrt hrt)
|
||||
add_subdirectory(../imxrt/led_pwm led_pwm)
|
||||
add_subdirectory(../imxrt/io_pins io_pins)
|
||||
add_subdirectory(../imxrt/tone_alarm tone_alarm)
|
||||
add_subdirectory(../imxrt/version version)
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/adc.h"
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/io_timer.h"
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <px4_platform/micro_hal.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_NXPIMXRT1062
|
||||
|
||||
#// Fixme: using ??
|
||||
#define PX4_BBSRAM_SIZE 2048
|
||||
#define PX4_BBSRAM_GETDESC_IOCTL 0
|
||||
#define PX4_NUMBER_I2C_BUSES 4
|
||||
|
||||
#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE
|
||||
#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO
|
||||
|
||||
#include <chip.h>
|
||||
#include <imxrt_lpspi.h>
|
||||
#include <imxrt_lpi2c.h>
|
||||
//# include <imxrt_uid.h> todo:Upsteam UID access
|
||||
|
||||
/* imxrt defines the 64 bit UUID as
|
||||
*
|
||||
* OCOTP 0x410 bits 31:0
|
||||
* OCOTP 0x420 bits 63:32
|
||||
*
|
||||
* PX4 uses the words in bigendian order MSB to LSB
|
||||
* word [0] [1]
|
||||
* bits 63-32, 31-00,
|
||||
*/
|
||||
#define PX4_CPU_UUID_BYTE_LENGTH 8
|
||||
#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t))
|
||||
|
||||
/* The mfguid will be an array of bytes with
|
||||
* MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1
|
||||
*
|
||||
* It will be converted to a string with the MSD on left and LSD on the right most position.
|
||||
*/
|
||||
#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH
|
||||
|
||||
/* define common formating across all commands */
|
||||
|
||||
#define PX4_CPU_UUID_WORD32_FORMAT "%08x"
|
||||
#define PX4_CPU_UUID_WORD32_SEPARATOR ":"
|
||||
|
||||
#define PX4_CPU_UUID_WORD32_UNIQUE_H 0 /* Least significant digits change the most (die wafer,X,Y */
|
||||
#define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Most significant digits change the least (lot#) */
|
||||
|
||||
/* Separator nnn:nnn:nnnn 2 char per byte term */
|
||||
#define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
|
||||
#define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
|
||||
|
||||
#define imxrt_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
|
||||
|
||||
#define px4_savepanic(fileno, context, length) imxrt_bbsram_savepanic(fileno, context, length)
|
||||
|
||||
/* bus_num is 1 based on imx and must be translated from the legacy one based */
|
||||
|
||||
#define PX4_BUS_OFFSET 0 /* imxrt buses are 1 based no adjustment needed */
|
||||
|
||||
#define px4_spibus_initialize(bus_num_1based) imxrt_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
|
||||
|
||||
#define px4_i2cbus_initialize(bus_num_1based) imxrt_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
|
||||
#define px4_i2cbus_uninitialize(pdev) imxrt_i2cbus_uninitialize(pdev)
|
||||
|
||||
#define px4_arch_configgpio(pinset) imxrt_config_gpio(pinset)
|
||||
#define px4_arch_unconfiggpio(pinset)
|
||||
#define px4_arch_gpioread(pinset) imxrt_gpio_read(pinset)
|
||||
#define px4_arch_gpiowrite(pinset, value) imxrt_gpio_write(pinset, value)
|
||||
|
||||
/* imxrt_gpiosetevent is not implemented and will need to be added */
|
||||
|
||||
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) imxrt_gpiosetevent(pinset,r,f,e,fp,a)
|
||||
|
||||
|
||||
__END_DECLS
|
||||
Loading…
Reference in New Issue