forked from Archive/PX4-Autopilot
Support for Raspberry PI RP2040 MCU (#18083)
This commit is contained in:
parent
8f6fd5f37b
commit
ea1ae73526
|
@ -79,6 +79,7 @@ pipeline {
|
|||
"nxp_ucans32k146_canbootloader",
|
||||
"nxp_ucans32k146_default",
|
||||
"omnibus_f4sd_default",
|
||||
"raspberrypi_pico_default",
|
||||
"px4_fmu-v2_default",
|
||||
"px4_fmu-v2_fixedwing",
|
||||
"px4_fmu-v2_multicopter",
|
||||
|
|
|
@ -48,6 +48,7 @@ jobs:
|
|||
nxp_fmurt1062-v1,
|
||||
nxp_ucans32k146,
|
||||
omnibus_f4sd,
|
||||
raspberrypi_pico,
|
||||
px4_fmu-v2,
|
||||
px4_fmu-v3,
|
||||
px4_fmu-v4,
|
||||
|
|
|
@ -221,3 +221,8 @@ CONFIG:
|
|||
buildType: MinSizeRel
|
||||
settings:
|
||||
CONFIG: nxp_fmuk66-v3_default
|
||||
raspberrypi_pico_default:
|
||||
short: raspberrypi_pico
|
||||
buildType: MinSizeRel
|
||||
settings:
|
||||
CONFIG: raspberrypi_pico_default
|
||||
|
|
|
@ -136,5 +136,6 @@
|
|||
"workbench.settings.enableNaturalLanguageSearch": false,
|
||||
"yaml.schemas": {
|
||||
"${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
|
||||
}
|
||||
},
|
||||
"cortex-debug.openocdPath": "${env:PICO_SDK_PATH}/../openocd/src/openocd" // Added for rp2040
|
||||
}
|
||||
|
|
|
@ -0,0 +1,52 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m0plus"
|
||||
CONFIG_BOARD_CONSTRAINED_FLASH=y
|
||||
CONFIG_BOARD_CONSTRAINED_MEMORY=y
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_BMP280=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=y
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||
CONFIG_SYSTEMCMDS_ESC_CALIB=y
|
||||
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_MIXER=y
|
||||
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_NSHTERM=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_PWM=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
|
@ -0,0 +1,13 @@
|
|||
{
|
||||
"board_id": 42,
|
||||
"magic": "RASPBERRYPIPICO",
|
||||
"description": "Firmware for the raspberry pi Pico board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "RaspberrypiPico",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 1032192,
|
||||
"git_identity": "",
|
||||
"board_revision": 0
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# system_power unavailable
|
||||
param set-default CBRK_SUPPLY_CHK 894281
|
||||
|
||||
# Disable safety switch by default
|
||||
param set-default CBRK_IO_SAFETY 22027
|
||||
|
||||
# use the Q attitude estimator, it works w/o mag or GPS.
|
||||
# param set-default SYS_MC_EST_GROUP 3
|
||||
# param set-default ATT_ACC_COMP 0
|
||||
# param set-default ATT_W_ACC 0.4000
|
||||
# param set-default ATT_W_GYRO_BIAS 0.0000
|
||||
|
||||
# param set-default SYS_HAS_MAG 0
|
|
@ -0,0 +1,7 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -0,0 +1,12 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific sensors init
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
board_adc start
|
||||
|
||||
# try starting an mpu9250 IMU on external SPI
|
||||
mpu9250 start -S
|
||||
|
||||
# try starting a bmp280 barometer on external SPI
|
||||
bmp280 start -S
|
|
@ -0,0 +1,25 @@
|
|||
#
|
||||
# For a description of the syntax of this configuration file,
|
||||
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||
#
|
||||
|
||||
config RP2040_FLASH_BOOT
|
||||
bool "flash boot"
|
||||
default y
|
||||
---help---
|
||||
If y, the built binary can be used for flash boot.
|
||||
If not, the binary is for SRAM boot.
|
||||
|
||||
config RP2040_FLASH_CHIP
|
||||
string "flash chip name"
|
||||
default "w25q080"
|
||||
---help---
|
||||
Name of NOR flash device connected to RP2040 SoC.
|
||||
(Used to choose the secondary boot loader.)
|
||||
Basically this option should not be changed.
|
||||
|
||||
config RP2040_UF2_BINARY
|
||||
bool "uf2 binary format"
|
||||
default y
|
||||
---help---
|
||||
Create nuttx.uf2 binary format used on RP2040 based arch.
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __ARCH_BOARD_BOARD_H
|
||||
#define __ARCH_BOARD_BOARD_H
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#ifndef __ASSEMBLY__
|
||||
# include <stdint.h>
|
||||
#endif
|
||||
|
||||
/* Clocking *****************************************************************/
|
||||
|
||||
#define MHZ 1000000
|
||||
|
||||
#define BOARD_XOSC_FREQ (12 * MHZ)
|
||||
#define BOARD_PLL_SYS_FREQ (125 * MHZ)
|
||||
#define BOARD_PLL_USB_FREQ (48 * MHZ)
|
||||
|
||||
#define BOARD_REF_FREQ (12 * MHZ)
|
||||
#define BOARD_SYS_FREQ (125 * MHZ)
|
||||
#define BOARD_PERI_FREQ (125 * MHZ)
|
||||
#define BOARD_USB_FREQ (48 * MHZ)
|
||||
#define BOARD_ADC_FREQ (48 * MHZ)
|
||||
#define BOARD_RTC_FREQ 46875
|
||||
|
||||
#define BOARD_UART_BASEFREQ BOARD_PERI_FREQ
|
||||
|
||||
#define BOARD_TICK_CLOCK (1 * MHZ)
|
||||
|
||||
/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the
|
||||
* omnibusf4sd. The following definitions describe how NuttX controls the LEDs:
|
||||
*/
|
||||
|
||||
// #define LED_STARTED 0 /* LED1 */
|
||||
// #define LED_HEAPALLOCATE 1 /* LED2 */
|
||||
// #define LED_IRQSENABLED 2 /* LED1 */
|
||||
// #define LED_STACKCREATED 3 /* LED1 + LED2 */
|
||||
// #define LED_INIRQ 4 /* LED1 */
|
||||
// #define LED_SIGNAL 5 /* LED2 */
|
||||
// #define LED_ASSERTION 6 /* LED1 + LED2 */
|
||||
// #define LED_PANIC 7 /* LED1 + LED2 */
|
||||
|
||||
/* Alternate function pin selections ************************************************/
|
||||
|
||||
/*
|
||||
* UARTs.
|
||||
* UART0TX: GPIO0
|
||||
* UART0RX: GPIO1
|
||||
* UART1TX: GPIO8
|
||||
* UART1RX: GPIO9
|
||||
*/
|
||||
#define CONFIG_RP2040_UART0_GPIO 0 /* TELEM */
|
||||
|
||||
#define CONFIG_RP2040_UART1_GPIO 8 /* GPS */
|
||||
|
||||
/*
|
||||
* I2C (external)
|
||||
*
|
||||
* I2C1SCL: GPIO7
|
||||
* I2C1SDA: GPIO6
|
||||
*
|
||||
* TODO:
|
||||
* The optional _GPIO configurations allow the I2C driver to manually
|
||||
* reset the bus to clear stuck slaves. They match the pin configuration,
|
||||
* but are normally-high GPIOs.
|
||||
*/
|
||||
#define CONFIG_RP2040_I2C1_GPIO 6
|
||||
|
||||
/* SPI0:
|
||||
* SPIDEV_FLASH (probably micro sd card)
|
||||
* CS: GPIO5 -- should be configured in sec/spi.cpp (probably)
|
||||
* CLK: GPIO2
|
||||
* MISO: GPIO4
|
||||
* MOSI: GPIO3
|
||||
*/
|
||||
|
||||
#define GPIO_SPI0_SCLK ( 2 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
#define GPIO_SPI0_MISO ( 4 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
#define GPIO_SPI0_MOSI ( 3 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
|
||||
/* SPI1:
|
||||
* MPU9250 and BMP280
|
||||
* CS: GPIO13 for MPU9250, GPIO14 for BMP280 -- should be configured in sec/spi.cpp (probably)
|
||||
* CLK: GPIO10
|
||||
* MISO: GPIO12
|
||||
* MOSI: GPIO11
|
||||
*/
|
||||
|
||||
#define GPIO_SPI1_SCLK ( 10 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
#define GPIO_SPI1_MISO ( 12 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
#define GPIO_SPI1_MOSI ( 11 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) )
|
||||
|
||||
|
||||
#endif /* __ARCH_BOARD_BOARD_H */
|
|
@ -0,0 +1,118 @@
|
|||
#
|
||||
# This file is autogenerated: PLEASE DO NOT EDIT IT.
|
||||
#
|
||||
# You can use "make menuconfig" to make any modifications to the installed .config file.
|
||||
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
|
||||
# modifications.
|
||||
#
|
||||
# CONFIG_DEV_CONSOLE is not set
|
||||
# CONFIG_RP2040_SPI_DRIVER is not set
|
||||
CONFIG_ARCH="arm"
|
||||
CONFIG_ARCH_BOARD_CUSTOM=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="rp2040"
|
||||
CONFIG_ARCH_CHIP_RP2040=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=512
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=10450
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_PRODUCTSTR="PX4 RaspberryPi Pico"
|
||||
CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=2000
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEV_FIFO_SIZE=0
|
||||
CONFIG_DEV_PIPE_SIZE=70
|
||||
CONFIG_DISABLE_MQUEUE=y
|
||||
CONFIG_DISABLE_POSIX_TIMERS=y
|
||||
CONFIG_FAT_DMAMEMORY=y
|
||||
CONFIG_FAT_LCNAMES=y
|
||||
CONFIG_FAT_LFN=y
|
||||
CONFIG_FAT_LFN_ALIAS_HASH=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_FS_BINFS=y
|
||||
CONFIG_FS_CROMFS=y
|
||||
CONFIG_FS_FAT=y
|
||||
CONFIG_FS_FATTIME=y
|
||||
CONFIG_FS_PROCFS=y
|
||||
CONFIG_FS_PROCFS_REGISTER=y
|
||||
CONFIG_FS_ROMFS=y
|
||||
CONFIG_GRAN=y
|
||||
CONFIG_GRAN_INTR=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
CONFIG_NSH_ARCHINIT=y
|
||||
CONFIG_NSH_ARCHROMFS=y
|
||||
CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_CROMFSETC=y
|
||||
CONFIG_NSH_DISABLE_IFCONFIG=y
|
||||
CONFIG_NSH_DISABLE_IFUPDOWN=y
|
||||
CONFIG_NSH_DISABLE_REBOOT=y
|
||||
CONFIG_NSH_DISABLE_TELNETD=y
|
||||
CONFIG_NSH_FATDEVNO=0
|
||||
CONFIG_NSH_FILEIOSIZE=512
|
||||
CONFIG_NSH_LINELEN=128
|
||||
CONFIG_NSH_MAXARGUMENTS=15
|
||||
CONFIG_NSH_NESTDEPTH=8
|
||||
CONFIG_NSH_READLINE=y
|
||||
CONFIG_NSH_ROMFSETC=y
|
||||
CONFIG_NSH_ROMFSSECTSIZE=128
|
||||
CONFIG_NSH_STRERROR=y
|
||||
CONFIG_NSH_USBCONSOLE=y
|
||||
CONFIG_NSH_VARS=y
|
||||
CONFIG_PIPES=y
|
||||
CONFIG_PREALLOC_TIMERS=50
|
||||
CONFIG_PRIORITY_INHERITANCE=y
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAM_SIZE=270336
|
||||
CONFIG_RAM_START=0x20000000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_RP2040_I2C1=y
|
||||
CONFIG_RP2040_I2C=y
|
||||
CONFIG_RP2040_SPI0=y
|
||||
CONFIG_RP2040_SPI1=y
|
||||
CONFIG_RP2040_SPI=y
|
||||
CONFIG_RP2040_UART1=y
|
||||
CONFIG_SCHED_ATEXIT=y
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_HPWORKPRIORITY=249
|
||||
CONFIG_SCHED_HPWORKSTACKSIZE=1280
|
||||
CONFIG_SCHED_INSTRUMENTATION=y
|
||||
CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
|
||||
CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SEM_NNESTPRIO=8
|
||||
CONFIG_SEM_PREALLOCHOLDERS=0
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
CONFIG_SIG_SIGUSR1_ACTION=y
|
||||
CONFIG_SIG_SIGUSR2_ACTION=y
|
||||
CONFIG_SIG_SIGWORK=4
|
||||
CONFIG_STACK_COLORATION=y
|
||||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_BUFFER_SIZE=32
|
||||
CONFIG_SYSTEM_CDCACM=y
|
||||
CONFIG_SYSTEM_NSH=y
|
||||
CONFIG_TASK_NAME_SIZE=24
|
||||
CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
CONFIG_USEC_PER_TICK=1000
|
||||
CONFIG_USERMAIN_STACKSIZE=2944
|
||||
CONFIG_USER_ENTRYPOINT="nsh_main"
|
||||
CONFIG_WATCHDOG=y
|
|
@ -0,0 +1,113 @@
|
|||
/****************************************************************************
|
||||
* boards/arm/rp2040/raspberrypi-pico/scripts/raspberrypi-pico-flash.ld
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x10000000, LENGTH = 2048K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 264K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
EXTERN(_vectors)
|
||||
ENTRY(_stext)
|
||||
EXTERN(bootStr)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.flash_begin : {
|
||||
__flash_binary_start = .;
|
||||
} > flash
|
||||
|
||||
.boot2 : {
|
||||
__boot2_start__ = .;
|
||||
KEEP (*(.boot2))
|
||||
__boot2_end__ = .;
|
||||
} > flash
|
||||
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.glue_7)
|
||||
*(.glue_7t)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP (*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.ram_vectors (COPY) : {
|
||||
*(.ram_vectors)
|
||||
} > sram
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
. = ALIGN(4);
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
|
@ -0,0 +1,113 @@
|
|||
/****************************************************************************
|
||||
* boards/arm/rp2040/raspberrypi-pico/scripts/raspberrypi-pico-flash.ld
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x10000000, LENGTH = 2048K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 264K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
EXTERN(_vectors)
|
||||
ENTRY(_stext)
|
||||
EXTERN(bootStr)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.flash_begin : {
|
||||
__flash_binary_start = .;
|
||||
} > flash
|
||||
|
||||
.boot2 : {
|
||||
__boot2_start__ = .;
|
||||
KEEP (*(.boot2))
|
||||
__boot2_end__ = .;
|
||||
} > flash
|
||||
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.glue_7)
|
||||
*(.glue_7t)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP (*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.ram_vectors (COPY) : {
|
||||
*(.ram_vectors)
|
||||
} > sram
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
. = ALIGN(4);
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
|
@ -0,0 +1,52 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_library(drivers_board
|
||||
boot_string.c
|
||||
i2c.cpp
|
||||
init.c
|
||||
led.c
|
||||
spi.cpp
|
||||
timer_config.cpp
|
||||
usb.c
|
||||
)
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
PRIVATE
|
||||
arch_spi
|
||||
drivers__led # drv_led_start
|
||||
nuttx_arch # sdio
|
||||
nuttx_drivers # sdio
|
||||
px4_layer
|
||||
arch_io_pins
|
||||
)
|
|
@ -0,0 +1,140 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file board_config.h
|
||||
*
|
||||
* board internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* LEDs */
|
||||
// LED1 - GPIO 25 - Green
|
||||
#define GPIO_LED1 PX4_MAKE_GPIO_OUTPUT_CLEAR(25) // Take a look at rpi_common micro_hal.h
|
||||
#define GPIO_LED_BLUE GPIO_LED1
|
||||
|
||||
#define BOARD_OVERLOAD_LED LED_BLUE
|
||||
|
||||
/*
|
||||
* ADC channels
|
||||
*
|
||||
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
|
||||
*/
|
||||
#define ADC_CHANNELS (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) // Change this later based on the adc channels actually used
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 1 // Corresponding GPIO 27. Used in init.c for disabling GPIO_IE
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL 2 // Corresponding GPIO 28. Used in init.c for disabling GPIO_IE
|
||||
#define ADC_RC_RSSI_CHANNEL 0
|
||||
|
||||
/* Define Battery 1 Voltage Divider and A per V. */
|
||||
#define BOARD_BATTERY1_V_DIV (13.653333333f)
|
||||
#define BOARD_BATTERY1_A_PER_V (36.367515152f)
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 1
|
||||
#define HRT_TIMER_CHANNEL 1
|
||||
#define HRT_PPM_CHANNEL 1 // Number really doesn't matter for this board
|
||||
#define GPIO_PPM_IN (16 | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
|
||||
#define RC_SERIAL_PORT "/dev/ttyS3"
|
||||
#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
|
||||
|
||||
/* This board provides a DMA pool and APIs */ // Needs to be figured out
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 2048
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
#define BOARD_CONSOLE_BUFFER_SIZE (1024*3)
|
||||
|
||||
/* USB
|
||||
*
|
||||
* VBUS detection is on 29 ADC_DPM0 and PTE8
|
||||
*/
|
||||
#define GPIO_USB_VBUS_VALID (24 | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) // Used in usb.c
|
||||
|
||||
/* PWM
|
||||
*
|
||||
* Alternatively CH3/CH4 could be assigned to UART6_TX/RX
|
||||
*/
|
||||
#define DIRECT_PWM_OUTPUT_CHANNELS 4
|
||||
|
||||
// Has pwm outputs
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/*
|
||||
* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
|
||||
* this board support the ADC system_power interface, and therefore
|
||||
* provides the true logic GPIO BOARD_ADC_xxxx macros.
|
||||
*/
|
||||
|
||||
#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_USB_VBUS_VALID))
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: rp2040_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void rp2040_spiinitialize(void);
|
||||
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: rp2040_usbinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure USB IO.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void rp2040_usbinitialize(void);
|
||||
|
||||
extern void board_peripheral_reset(int ms);
|
||||
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,20 @@
|
|||
#include <stdint.h>
|
||||
|
||||
__attribute__((section(".boot2")))
|
||||
const char bootStr[256] = {0x00, 0xb5, 0x32, 0x4b, 0x21, 0x20, 0x58, 0x60, 0x98, 0x68, 0x02, 0x21, 0x88, 0x43, 0x98, 0x60,
|
||||
0xd8, 0x60, 0x18, 0x61, 0x58, 0x61, 0x2e, 0x4b, 0x00, 0x21, 0x99, 0x60, 0x02, 0x21, 0x59, 0x61,
|
||||
0x01, 0x21, 0xf0, 0x22, 0x99, 0x50, 0x2b, 0x49, 0x19, 0x60, 0x01, 0x21, 0x99, 0x60, 0x35, 0x20,
|
||||
0x00, 0xf0, 0x44, 0xf8, 0x02, 0x22, 0x90, 0x42, 0x14, 0xd0, 0x06, 0x21, 0x19, 0x66, 0x00, 0xf0,
|
||||
0x34, 0xf8, 0x19, 0x6e, 0x01, 0x21, 0x19, 0x66, 0x00, 0x20, 0x18, 0x66, 0x1a, 0x66, 0x00, 0xf0,
|
||||
0x2c, 0xf8, 0x19, 0x6e, 0x19, 0x6e, 0x19, 0x6e, 0x05, 0x20, 0x00, 0xf0, 0x2f, 0xf8, 0x01, 0x21,
|
||||
0x08, 0x42, 0xf9, 0xd1, 0x00, 0x21, 0x99, 0x60, 0x1b, 0x49, 0x19, 0x60, 0x00, 0x21, 0x59, 0x60,
|
||||
0x1a, 0x49, 0x1b, 0x48, 0x01, 0x60, 0x01, 0x21, 0x99, 0x60, 0xeb, 0x21, 0x19, 0x66, 0xa0, 0x21,
|
||||
0x19, 0x66, 0x00, 0xf0, 0x12, 0xf8, 0x00, 0x21, 0x99, 0x60, 0x16, 0x49, 0x14, 0x48, 0x01, 0x60,
|
||||
0x01, 0x21, 0x99, 0x60, 0x01, 0xbc, 0x00, 0x28, 0x00, 0xd0, 0x00, 0x47, 0x12, 0x48, 0x13, 0x49,
|
||||
0x08, 0x60, 0x03, 0xc8, 0x80, 0xf3, 0x08, 0x88, 0x08, 0x47, 0x03, 0xb5, 0x99, 0x6a, 0x04, 0x20,
|
||||
0x01, 0x42, 0xfb, 0xd0, 0x01, 0x20, 0x01, 0x42, 0xf8, 0xd1, 0x03, 0xbd, 0x02, 0xb5, 0x18, 0x66,
|
||||
0x18, 0x66, 0xff, 0xf7, 0xf2, 0xff, 0x18, 0x6e, 0x18, 0x6e, 0x02, 0xbd, 0x00, 0x00, 0x02, 0x40,
|
||||
0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x07, 0x00, 0x00, 0x03, 0x5f, 0x00, 0x21, 0x22, 0x00, 0x00,
|
||||
0xf4, 0x00, 0x00, 0x18, 0x22, 0x20, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x10, 0x08, 0xed, 0x00, 0xe0,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x74, 0xb2, 0x4e, 0x7a
|
||||
};
|
|
@ -0,0 +1,38 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/i2c_hw_description.h>
|
||||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(2),
|
||||
};
|
|
@ -0,0 +1,399 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file init.c
|
||||
*
|
||||
* board specific early startup code. This file implements the
|
||||
* board_app_initialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialization.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <nuttx/i2c/i2c_master.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
#include <nuttx/mm/gran.h>
|
||||
|
||||
#include "board_config.h"
|
||||
#include <rp2040_uart.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_platform_common/init.h>
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
# if defined(FLASH_BASED_PARAMS)
|
||||
# include <parameters/flashparams/flashfs.h>
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Ideally we'd be able to get these from arm_internal.h,
|
||||
* but since we want to be able to disable the NuttX use
|
||||
* of leds for system indication at will and there is no
|
||||
* separate switch, we need to build independent of the
|
||||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init(void);
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
__END_DECLS
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/************************************************************************************
|
||||
* Name: board_peripheral_reset
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
************************************************************************************/
|
||||
__EXPORT void board_peripheral_reset(int ms)
|
||||
{
|
||||
UNUSED(ms);
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_on_reset
|
||||
*
|
||||
* Description:
|
||||
* Optionally provided function called on entry to board_system_reset
|
||||
* It should perform any house keeping prior to the rest.
|
||||
*
|
||||
* status - 1 if resetting to boot loader
|
||||
* 0 if just resetting
|
||||
*
|
||||
************************************************************************************/
|
||||
__EXPORT void board_on_reset(int status)
|
||||
{
|
||||
// Configure the GPIO pins to outputs and keep them low.
|
||||
for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
|
||||
px4_arch_configgpio(io_timer_channel_get_gpio_output(i));
|
||||
}
|
||||
|
||||
/*
|
||||
* On resets invoked from system (not boot) insure we establish a low
|
||||
* output state (discharge the pins) on PWM pins before they become inputs.
|
||||
*/
|
||||
|
||||
if (status >= 0) {
|
||||
up_mdelay(400);
|
||||
}
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_read_VBUS_state
|
||||
*
|
||||
* Description:
|
||||
* All boards must provide a way to read the state of VBUS, this my be simple
|
||||
* digital input on a GPIO. Or something more complicated like a Analong input
|
||||
* or reading a bit from a USB controller register.
|
||||
*
|
||||
* Returns - 0 if connected.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int board_read_VBUS_state(void)
|
||||
{
|
||||
return BOARD_ADC_USB_CONNECTED ? 0 : 1;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: rp2040_boardearlyinitialize
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* This function is taken directly from nuttx's rp2040_boardinitialize.c
|
||||
****************************************************************************/
|
||||
|
||||
void rp2040_boardearlyinitialize(void)
|
||||
{
|
||||
/* Set default UART pin */
|
||||
#if defined(CONFIG_RP2040_UART0) && CONFIG_RP2040_UART0_GPIO >= 0
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO, RP2040_GPIO_FUNC_UART); /* TX */
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 1, RP2040_GPIO_FUNC_UART); /* RX */
|
||||
# ifdef CONFIG_SERIAL_OFLOWCONTROL
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 2, RP2040_GPIO_FUNC_UART); /* CTS */
|
||||
# endif /* CONFIG_SERIAL_OFLOWCONTROL */
|
||||
# ifdef CONFIG_SERIAL_IFLOWCONTROL
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 3, RP2040_GPIO_FUNC_UART); /* RTS */
|
||||
# endif /* CONFIG_SERIAL_IFLOWCONTROL */
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_RP2040_UART1) && CONFIG_RP2040_UART1_GPIO >= 0
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO, RP2040_GPIO_FUNC_UART); /* TX */
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 1, RP2040_GPIO_FUNC_UART); /* RX */
|
||||
# ifdef CONFIG_SERIAL_OFLOWCONTROL
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 2, RP2040_GPIO_FUNC_UART); /* CTS */
|
||||
# endif /* CONFIG_SERIAL_OFLOWCONTROL */
|
||||
# ifdef CONFIG_SERIAL_IFLOWCONTROL
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 3, RP2040_GPIO_FUNC_UART); /* RTS */
|
||||
# endif /* CONFIG_SERIAL_IFLOWCONTROL */
|
||||
#endif
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: rp2040_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All architectures must provide the following entry point. This entry point
|
||||
* is called early in the initialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void
|
||||
rp2040_boardinitialize(void)
|
||||
{
|
||||
// /* Reset all PWM to Low outputs */
|
||||
// board_on_reset(-1);
|
||||
|
||||
// /* configure LEDs */
|
||||
board_autoled_initialize();
|
||||
|
||||
// Disable IE and enable OD on GPIO 26-29 (These are ADC Pins)
|
||||
// Do this only for the channels configured in board_config.h
|
||||
rp2040_gpioconfig(27 | GPIO_FUN(RP2040_GPIO_FUNC_NULL)); /* BATT_VOLTAGE_SENS */
|
||||
clrbits_reg32(RP2040_PADS_BANK0_GPIO_IE, RP2040_PADS_BANK0_GPIO(27)); /* BATT_VOLTAGE_SENS */
|
||||
setbits_reg32(RP2040_PADS_BANK0_GPIO_OD, RP2040_PADS_BANK0_GPIO(27)); /* BATT_VOLTAGE_SENS */
|
||||
rp2040_gpioconfig(28 | GPIO_FUN(RP2040_GPIO_FUNC_NULL)); /* BATT_VOLTAGE_SENS */
|
||||
clrbits_reg32(RP2040_PADS_BANK0_GPIO_IE, RP2040_PADS_BANK0_GPIO(28)); /* BATT_CURRENT_SENS */
|
||||
setbits_reg32(RP2040_PADS_BANK0_GPIO_OD, RP2040_PADS_BANK0_GPIO(28)); /* BATT_CURRENT_SENS */
|
||||
|
||||
/* Set default I2C pin */
|
||||
#if defined(CONFIG_RP2040_I2C0) && CONFIG_RP2040_I2C0_GPIO >= 0
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_I2C0_GPIO, RP2040_GPIO_FUNC_I2C); /* SDA */
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_I2C0_GPIO + 1, RP2040_GPIO_FUNC_I2C); /* SCL */
|
||||
rp2040_gpio_set_pulls(CONFIG_RP2040_I2C0_GPIO, true, false); /* Pull up */
|
||||
rp2040_gpio_set_pulls(CONFIG_RP2040_I2C0_GPIO + 1, true, false);
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_RP2040_I2C1) && CONFIG_RP2040_I2C1_GPIO >= 0
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_I2C1_GPIO, RP2040_GPIO_FUNC_I2C); /* SDA */
|
||||
rp2040_gpio_set_function(CONFIG_RP2040_I2C1_GPIO + 1, RP2040_GPIO_FUNC_I2C); /* SCL */
|
||||
rp2040_gpio_set_pulls(CONFIG_RP2040_I2C1_GPIO, true, false); /* Pull up */
|
||||
rp2040_gpio_set_pulls(CONFIG_RP2040_I2C1_GPIO + 1, true, false);
|
||||
#endif
|
||||
|
||||
// // TODO: power peripherals
|
||||
// ///* configure power supply control/sense pins */
|
||||
// //stm32_configgpio(GPIO_PERIPH_3V3_EN);
|
||||
// //stm32_configgpio(GPIO_VDD_BRICK_VALID);
|
||||
// //stm32_configgpio(GPIO_VDD_USB_VALID);
|
||||
|
||||
// // TODO: 3v3 Sensor?
|
||||
// ///* Start with Sensor voltage off We will enable it
|
||||
// // * in board_app_initialize
|
||||
// // */
|
||||
// //stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
|
||||
|
||||
// // TODO: SBUS inversion? SPEK power?
|
||||
// //stm32_configgpio(GPIO_SBUS_INV);
|
||||
// //stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
|
||||
|
||||
// // TODO: $$$ Unused?
|
||||
// //stm32_configgpio(GPIO_8266_GPIO0);
|
||||
// //stm32_configgpio(GPIO_8266_PD);
|
||||
// //stm32_configgpio(GPIO_8266_RST);
|
||||
|
||||
// /* Safety - led don in led driver */
|
||||
|
||||
// // TODO: unused?
|
||||
// //stm32_configgpio(GPIO_BTN_SAFETY);
|
||||
|
||||
// // TODO: RSSI
|
||||
// //stm32_configgpio(GPIO_RSSI_IN);
|
||||
|
||||
// stm32_configgpio(GPIO_PPM_IN);
|
||||
|
||||
/* configure SPI all interfaces GPIO */
|
||||
rp2040_spiinitialize();
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_app_initialize
|
||||
*
|
||||
* Description:
|
||||
* Perform application specific initialization. This function is never
|
||||
* called directly from application code, but only indirectly via the
|
||||
* (non-standard) boardctl() interface using the command BOARDIOC_INIT.
|
||||
*
|
||||
* Input Parameters:
|
||||
* arg - The boardctl() argument is passed to the board_app_initialize()
|
||||
* implementation without modification. The argument has no
|
||||
* meaning to NuttX; the meaning of the argument is a contract
|
||||
* between the board-specific initalization logic and the the
|
||||
* matching application logic. The value cold be such things as a
|
||||
* mode enumeration value, a set of DIP switch switch settings, a
|
||||
* pointer to configuration data read from a file or serial FLASH,
|
||||
* or whatever you would like to do with it. Every implementation
|
||||
* should accept zero/NULL as a default configuration.
|
||||
*
|
||||
* Returned Value:
|
||||
* Zero (OK) is returned on success; a negated errno value is returned on
|
||||
* any failure to indicate the nature of the failure.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
// static struct spi_dev_s *spi1;
|
||||
static struct spi_dev_s *spi2;
|
||||
|
||||
__EXPORT int board_app_initialize(uintptr_t arg)
|
||||
{
|
||||
px4_platform_init();
|
||||
|
||||
/* configure the DMA allocator */ // Needs to be figured out
|
||||
|
||||
if (board_dma_alloc_init() < 0) {
|
||||
syslog(LOG_ERR, "DMA alloc FAILED\n");
|
||||
}
|
||||
|
||||
/* set up the serial DMA polling */ // RP2040 nuttx implementation doesn't have serial_dma_poll function yet.
|
||||
// static struct hrt_call serial_dma_call;
|
||||
// struct timespec ts;
|
||||
|
||||
// /*
|
||||
// * Poll at 1ms intervals for received bytes that have not triggered
|
||||
// * a DMA event.
|
||||
// */
|
||||
// ts.tv_sec = 0;
|
||||
// ts.tv_nsec = 1000000;
|
||||
|
||||
// hrt_call_every(&serial_dma_call,
|
||||
// ts_to_abstime(&ts),
|
||||
// ts_to_abstime(&ts),
|
||||
// (hrt_callout)stm32_serial_dma_poll,
|
||||
// NULL);
|
||||
|
||||
/* initial LED state */
|
||||
drv_led_start();
|
||||
led_on(LED_BLUE);
|
||||
|
||||
// if (board_hardfault_init(2, true) != 0) { // Needs to be figured out as RP2040 doesn't have BBSRAM.
|
||||
// led_off(LED_BLUE);
|
||||
// }
|
||||
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
// // SPI1: SDCard // Will be configured later
|
||||
// /* Get the SPI port for the microSD slot */
|
||||
// spi1 = rp2040_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); // PX4_BUS_NUMBER_FROM_PX4(1)
|
||||
|
||||
// if (!spi1) {
|
||||
// syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO);
|
||||
// led_off(LED_BLUE);
|
||||
// return -ENODEV;
|
||||
// }
|
||||
|
||||
// /* Now bind the SPI interface to the MMCSD driver */
|
||||
// int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi1);
|
||||
|
||||
// if (result != OK) {
|
||||
// led_off(LED_BLUE);
|
||||
// syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n");
|
||||
// return -ENODEV;
|
||||
// }
|
||||
|
||||
// up_udelay(20);
|
||||
|
||||
// SPI2: MPU9250 and BMP280
|
||||
spi2 = rp2040_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(2));
|
||||
|
||||
if (!spi2) {
|
||||
syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n");
|
||||
led_off(LED_BLUE);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi2, 10000000);
|
||||
SPI_SETBITS(spi2, 8);
|
||||
SPI_SETMODE(spi2, SPIDEV_MODE3);
|
||||
up_udelay(20);
|
||||
|
||||
// #if defined(FLASH_BASED_PARAMS) // This probably doesn't relate to RP2040 right now.
|
||||
// static sector_descriptor_t params_sector_map[] = {
|
||||
// {1, 16 * 1024, 0x08004000},
|
||||
// {0, 0, 0},
|
||||
// };
|
||||
|
||||
// /* Initialize the flashfs layer to use heap allocated memory */
|
||||
// result = parameter_flashfs_init(params_sector_map, NULL, 0);
|
||||
|
||||
// if (result != OK) {
|
||||
// syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
|
||||
// led_off(LED_AMBER);
|
||||
// return -ENODEV;
|
||||
// }
|
||||
|
||||
// #endif
|
||||
|
||||
/* Configure the HW based on the manifest */
|
||||
|
||||
px4_platform_configure();
|
||||
|
||||
return OK;
|
||||
}
|
|
@ -0,0 +1,119 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file led.c
|
||||
*
|
||||
* board LED backend.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/*
|
||||
* Ideally we'd be able to get these from arm_internal.h,
|
||||
* but since we want to be able to disable the NuttX use
|
||||
* of leds for system indication at will and there is no
|
||||
* separate switch, we need to build independent of the
|
||||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init(void);
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
extern void led_toggle(int led);
|
||||
__END_DECLS
|
||||
|
||||
static uint32_t g_ledmap[] = {
|
||||
GPIO_LED_BLUE, // Onboard led on raspberrypi pico
|
||||
};
|
||||
|
||||
__EXPORT void led_init(void)
|
||||
{
|
||||
/* Configure LED GPIOs for output */
|
||||
for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
|
||||
px4_arch_configgpio(g_ledmap[l]);
|
||||
}
|
||||
}
|
||||
|
||||
static void phy_set_led(int led, bool state)
|
||||
{
|
||||
/* Pull Down to switch on */
|
||||
if (led == 0) {
|
||||
px4_arch_gpiowrite(g_ledmap[led], state);
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void led_on(int led)
|
||||
{
|
||||
phy_set_led(led, true);
|
||||
}
|
||||
|
||||
__EXPORT void led_off(int led)
|
||||
{
|
||||
phy_set_led(led, false);
|
||||
}
|
||||
|
||||
__EXPORT void led_toggle(int led)
|
||||
{
|
||||
if (led == 0) {
|
||||
phy_set_led(led, !px4_arch_gpioread(g_ledmap[led]));
|
||||
}
|
||||
}
|
||||
|
||||
// __EXPORT void board_autoled_initialize()
|
||||
// {
|
||||
// /* Configure LED1 GPIO for output */
|
||||
// px4_arch_configgpio(GPIO_LED1);
|
||||
// }
|
||||
|
||||
// __EXPORT void board_autoled_on(int led)
|
||||
// {
|
||||
// if (led == 1) {
|
||||
// /* Pull down to switch on */
|
||||
// px4_arch_gpiowrite(GPIO_LED1, false);
|
||||
// }
|
||||
// }
|
||||
|
||||
// __EXPORT void board_autoled_off(int led)
|
||||
// {
|
||||
// if (led == 1) {
|
||||
// /* Pull up to switch off */
|
||||
// px4_arch_gpiowrite(GPIO_LED1, true);
|
||||
// }
|
||||
// }
|
|
@ -0,0 +1,48 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/spi_hw_description.h>
|
||||
#include <drivers/drv_sensor.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::SPI0, {
|
||||
initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::Pin5}),
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI1, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Pin13}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Pin14}),
|
||||
}),
|
||||
};
|
||||
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses);
|
|
@ -0,0 +1,49 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/io_timer_hw_description.h>
|
||||
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOTimer(Timer::Timer1),
|
||||
initIOTimer(Timer::Timer2),
|
||||
};
|
||||
|
||||
constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
|
||||
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::ChannelA}, {GPIO::Pin18}),
|
||||
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::ChannelB}, {GPIO::Pin19}),
|
||||
initIOTimerChannel(io_timers, {Timer::Timer2, Timer::ChannelA}, {GPIO::Pin20}),
|
||||
initIOTimerChannel(io_timers, {Timer::Timer2, Timer::ChannelB}, {GPIO::Pin21}),
|
||||
};
|
||||
|
||||
constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers,
|
||||
timer_io_channels);
|
|
@ -0,0 +1,85 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file usb.c
|
||||
*
|
||||
* Board-specific USB functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/usb/usbdev.h>
|
||||
#include <nuttx/usb/usbdev_trace.h>
|
||||
|
||||
#include <arm_arch.h>
|
||||
#include "board_config.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Name: rp2040_usbinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to setup USB-related GPIO pins for the omnibusf4sd board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void rp2040_usbinitialize(void)
|
||||
{
|
||||
px4_arch_configgpio(GPIO_USB_VBUS_VALID);
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbsuspend
|
||||
*
|
||||
* Description:
|
||||
* Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
|
||||
* used. This function is called whenever the USB enters or leaves suspend mode.
|
||||
* This is an opportunity for the board logic to shutdown clocks, power, etc.
|
||||
* while the USB is suspended.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void rp2040_usbsuspend(FAR struct usbdev_s *dev, bool resume)
|
||||
{
|
||||
uinfo("resume: %d\n", resume);
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
set(cpu_flags "-mcpu=cortex-m0plus -mthumb -mfloat-abi=soft") # Nuttx toochain uses -mfloat-abi=soft
|
||||
|
||||
set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE)
|
||||
set(CMAKE_CXX_FLAGS "${cpu_flags}" CACHE STRING "" FORCE)
|
||||
set(CMAKE_ASM_FLAGS "${cpu_flags} -D__ASSEMBLY__" CACHE STRING "" FORCE)
|
|
@ -124,6 +124,9 @@ function(px4_os_determine_build_chip)
|
|||
elseif(CONFIG_ARCH_CHIP_S32K146)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "s32k14x")
|
||||
elseif(CONFIG_ARCH_CHIP_RP2040)
|
||||
set(CHIP_MANUFACTURER "rpi")
|
||||
set(CHIP "rp2040")
|
||||
else()
|
||||
message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.")
|
||||
endif()
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
add_subdirectory(${PX4_CHIP})
|
|
@ -0,0 +1,7 @@
|
|||
add_subdirectory(../rpi_common/adc adc)
|
||||
add_subdirectory(../rpi_common/hrt hrt)
|
||||
add_subdirectory(../rpi_common/version version)
|
||||
add_subdirectory(../rpi_common/board_reset board_reset)
|
||||
add_subdirectory(../rpi_common/board_critmon board_critmon)
|
||||
add_subdirectory(../rpi_common/spi spi)
|
||||
add_subdirectory(../rpi_common/io_pins io_pins)
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/adc.h"
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/hw_description.h"
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/i2c_hw_description.h"
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/io_timer.h"
|
|
@ -0,0 +1,36 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/io_timer_hw_description.h"
|
|
@ -0,0 +1,12 @@
|
|||
#pragma once
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/micro_hal.h"
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_UNUSED
|
||||
#define PX4_FLASH_BASE RP2040_FLASH_BASE
|
||||
#define PX4_NUMBER_I2C_BUSES 2
|
||||
#define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 4
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,66 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../rpi_common/include/px4_arch/spi_hw_description.h"
|
||||
|
||||
constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS])
|
||||
{
|
||||
const bool nuttx_enabled_spi_buses[] = {
|
||||
#ifdef CONFIG_RP2040_SPI0
|
||||
true,
|
||||
#else
|
||||
false,
|
||||
#endif
|
||||
#ifdef CONFIG_RP2040_SPI1
|
||||
true,
|
||||
#else
|
||||
false,
|
||||
#endif
|
||||
};
|
||||
|
||||
for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) {
|
||||
bool found_bus = false;
|
||||
|
||||
for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) {
|
||||
if (spi_busses_conf[j].bus == (int)i + 1) {
|
||||
found_bus = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured
|
||||
constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_RP2040_SPIx)");
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_adc
|
||||
adc.cpp
|
||||
)
|
|
@ -0,0 +1,176 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <board_config.h>
|
||||
#include <stdint.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_arch/adc.h>
|
||||
|
||||
// #include <rp2040_adc.h> Nuttx doesn't have this file in arch yet.
|
||||
#include <rp2040_gpio.h>
|
||||
|
||||
/*
|
||||
* Register accessors.
|
||||
* For now, no reason not to just use ADC1.
|
||||
*/
|
||||
#define REG(base, _reg) (*(volatile uint32_t *)((base) + (_reg)))
|
||||
|
||||
#define rCS(base) REG((base), 0x00) // ADC Control and Status
|
||||
#define rRESULT(base) REG((base), 0x04) // Result of most recent ADC conversion
|
||||
#define rFCS(base) REG((base), 0x08) // FIFO control and status
|
||||
#define rFIFO(base) REG((base), 0x0c) // Conversion result FIFO
|
||||
#define rDIV(base) REG((base), 0x10) // Clock divider
|
||||
#define rINTR(base) REG((base), 0x14) // Raw Interrupts
|
||||
#define rINTE(base) REG((base), 0x18) // Interrupt Enable
|
||||
#define rINTF(base) REG((base), 0x1c) // Interrupt Force
|
||||
#define rINTS(base) REG((base), 0x20) // Interrupt status after masking & forcing
|
||||
|
||||
int px4_arch_adc_init(uint32_t base_address)
|
||||
{
|
||||
/* Perform ADC init once per ADC */
|
||||
|
||||
static uint32_t once[SYSTEM_ADC_COUNT] {};
|
||||
|
||||
uint32_t *free = nullptr;
|
||||
|
||||
for (uint32_t i = 0; i < SYSTEM_ADC_COUNT; i++) {
|
||||
if (once[i] == base_address) {
|
||||
|
||||
/* This one was done already */
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* Use first free slot */
|
||||
|
||||
if (free == nullptr && once[i] == 0) {
|
||||
free = &once[i];
|
||||
}
|
||||
}
|
||||
|
||||
if (free == nullptr) {
|
||||
|
||||
/* ADC misconfigured SYSTEM_ADC_COUNT too small */;
|
||||
|
||||
PANIC();
|
||||
}
|
||||
|
||||
*free = base_address;
|
||||
|
||||
// Assuming that the ADC gpio is configured correctly,
|
||||
// all that is left to do is divide 48MHz clock if
|
||||
// necessary and then enable the ADC. (One reading
|
||||
// requires about 100 clocks.) Also enable the temp
|
||||
// sensor channel.
|
||||
|
||||
// Divide incoming 48MHz clock
|
||||
// (Trigger adc once per n+1 cycles)
|
||||
// So, n >= 96. Because, 1 sample = 96 clocks
|
||||
rDIV(base_address) = 0 | (0 << 8); // 8-bit fraction value | 16-bit int value => n = int + frac/256
|
||||
|
||||
// Enable temperature sensor and enable ADC
|
||||
rCS(base_address) = 1 | (1 << 1);
|
||||
px4_usleep(10);
|
||||
|
||||
// Select temperature channel and kick off a sample and wait for it to complete
|
||||
rCS(base_address) &= ~(0b111 << 12); // Clear AINSEL
|
||||
rCS(base_address) |= PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL << 12; // Choose temperature channel
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
rCS(base_address) |= 1 << 2; // Start a single conversion
|
||||
|
||||
while (!(rCS(base_address) & (1 << 8))) { // Check if the sample is ready
|
||||
|
||||
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
|
||||
if ((hrt_absolute_time() - now) > 500) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/* Read out result */
|
||||
(void) rRESULT(base_address);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void px4_arch_adc_uninit(uint32_t base_address)
|
||||
{
|
||||
// Disable ADC
|
||||
rCS(base_address) = 0;
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
/* run a single conversion right now - should take about 96 cycles (a few microseconds) max */
|
||||
rCS(base_address) &= ~(0b111 << 12); // Clear AINSEL
|
||||
rCS(base_address) |= channel << 12; // Choose temperature channel
|
||||
rCS(base_address) |= 1 << 2; // Start a single conversion
|
||||
|
||||
/* wait for the conversion to complete */
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
while (!(rCS(base_address) & (1 << 8))) { // Check if the sample is ready
|
||||
|
||||
/* 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) {
|
||||
px4_leave_critical_section(flags);
|
||||
return UINT32_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
/* read the result and clear EOC */
|
||||
uint32_t result = rRESULT(base_address);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float px4_arch_adc_reference_v()
|
||||
{
|
||||
return BOARD_ADC_POS_REF_V; // TODO: provide true vref
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_temp_sensor_mask()
|
||||
{
|
||||
|
||||
return 1 << PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL;
|
||||
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_dn_fullcount()
|
||||
{
|
||||
return 1 << 12; // 12 bit ADC
|
||||
}
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
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) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_board_reset
|
||||
board_reset.cpp
|
||||
)
|
|
@ -0,0 +1,158 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 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.cpp
|
||||
* Implementation of RP2040 based Board RESET API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
|
||||
// Functions in here are modified so that board_reset() function resembles
|
||||
// the one available in nuttx's boards/raspberrypi-pico folder.
|
||||
|
||||
#ifdef CONFIG_BOARDCTL_RESET
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_configure_reset
|
||||
*
|
||||
* Description:
|
||||
* Configures the device that maintains the state shared by the
|
||||
* application and boot loader. This is usually an RTC.
|
||||
*
|
||||
* Input Parameters:
|
||||
* mode - The type of reset. See reset_mode_e
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 for Success
|
||||
* 1 if invalid argument
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
// static const uint32_t modes[] = {
|
||||
// /* to tb */
|
||||
// /* BOARD_RESET_MODE_CLEAR 5 y */ 0,
|
||||
// /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb007b007,
|
||||
// /* BOARD_RESET_MODE_BOOT_TO_VALID_APP 0 y */ 0xb0070002,
|
||||
// /* BOARD_RESET_MODE_CAN_BL 10 n */ 0xb0080000,
|
||||
// /* BOARD_RESET_MODE_RTC_BOOT_FWOK 0 n */ 0xb0093a26
|
||||
// };
|
||||
|
||||
// int board_configure_reset(reset_mode_e mode, uint32_t arg)
|
||||
// {
|
||||
// int rv = -1;
|
||||
|
||||
// if (mode < arraySize(modes)) {
|
||||
|
||||
// stm32_pwr_enablebkp(true);
|
||||
|
||||
// arg = mode == BOARD_RESET_MODE_CAN_BL ? arg & ~0xff : 0;
|
||||
|
||||
// // Check if we can to use the new register definition
|
||||
|
||||
// #ifndef STM32_RTC_BK0R
|
||||
// *(uint32_t *)STM32_BKP_BASE = modes[mode] | arg;
|
||||
// #else
|
||||
// *(uint32_t *)STM32_RTC_BK0R = modes[mode] | arg;
|
||||
// #endif
|
||||
// stm32_pwr_enablebkp(false);
|
||||
// rv = OK;
|
||||
// }
|
||||
|
||||
// return rv;
|
||||
// }
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_reset
|
||||
*
|
||||
* Description:
|
||||
* Reset board. Support for this function is required by board-level
|
||||
* logic if CONFIG_BOARDCTL_RESET is selected.
|
||||
*
|
||||
* Input Parameters:
|
||||
* status - Status information provided with the reset event. This
|
||||
* meaning of this status information is board-specific. If not
|
||||
* used by a board, the value zero may be provided in calls to
|
||||
* board_reset().
|
||||
*
|
||||
* Returned Value:
|
||||
* If this function returns, then it was not possible to power-off the
|
||||
* board due to some constraints. The return value int this case is a
|
||||
* board-specific reason for the failure to shutdown.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int board_reset(int status)
|
||||
{
|
||||
if (status == 1) {
|
||||
// board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
|
||||
}
|
||||
|
||||
#if defined(BOARD_HAS_ON_RESET)
|
||||
// board_on_reset(status);
|
||||
#endif
|
||||
|
||||
up_systemreset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif /* CONFIG_BOARDCTL_RESET */
|
||||
|
||||
#if defined(SUPPORT_ALT_CAN_BOOTLOADER)
|
||||
/****************************************************************************
|
||||
* Name: board_booted_by_px4
|
||||
*
|
||||
* Description:
|
||||
* Determines if the the boot loader was PX4
|
||||
*
|
||||
* Input Parameters:
|
||||
* none
|
||||
*
|
||||
* Returned Value:
|
||||
* true if booted byt a PX4 bootloader.
|
||||
*
|
||||
****************************************************************************/
|
||||
bool board_booted_by_px4(void)
|
||||
{
|
||||
uint32_t *vectors = (uint32_t *) STM32_FLASH_BASE;
|
||||
|
||||
/* Nuttx uses common vector */
|
||||
|
||||
return (vectors[2] == vectors[3]) && (vectors[4] == vectors[5]);
|
||||
}
|
||||
#endif
|
|
@ -0,0 +1,41 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_hrt
|
||||
hrt.c
|
||||
)
|
||||
target_compile_options(arch_hrt
|
||||
PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
)
|
|
@ -0,0 +1,809 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_hrt.c
|
||||
*
|
||||
* High-resolution timer callouts and timekeeping.
|
||||
*
|
||||
* RP2040's internal 64 bit timer can be used for this purpose.
|
||||
*
|
||||
* 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 RP2040 driver per se; rather, we
|
||||
* claim the timer and then drive it directly.
|
||||
*/
|
||||
|
||||
#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 <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
||||
// #include "rp2040_gpio.h"
|
||||
|
||||
#ifdef CONFIG_DEBUG_HRT
|
||||
# define hrtinfo _info
|
||||
#else
|
||||
# define hrtinfo(x...)
|
||||
#endif
|
||||
|
||||
// RP2040 has a dedicated 64-bit timer which updates at 1MHz. This timer can be used here as hrt.
|
||||
// The advantage is that this timer will not overflow as it can run for thousands of years.
|
||||
// This timer is activated when the clk_ref is configured for watchdog and TICK is enabled.
|
||||
// Fortunately, nuttx by default does this for us in rp2040_clock.c file. Thus, no init required.
|
||||
// Four individual interrupts can be configured which are triggered when the lower 32-bits of the timer
|
||||
// matches with the value in ALARMx register. This allows for the interrupt to fire at ~72 min in future.
|
||||
// Take a look at src/drivers/drv_hrt.h to find all the necessary functions required to be implemented.
|
||||
|
||||
#ifdef HRT_TIMER
|
||||
|
||||
/* HRT configuration */
|
||||
#if HRT_TIMER == 1
|
||||
# define HRT_TIMER_BASE RP2040_TIMER_BASE
|
||||
#else
|
||||
# error HRT_TIMER must have a value of 1 because there is only one timer in RP2040
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Minimum/maximum deadlines.
|
||||
*
|
||||
* These are suitable for use with a 16-bit timer/counter clocked
|
||||
* at 1MHz. The high-resolution timer need only guarantee that it
|
||||
* not wrap more than once in the 50ms period for absolute time to
|
||||
* be consistently maintained.
|
||||
*
|
||||
* The minimum deadline must be such that the time taken between
|
||||
* reading a time and writing a deadline to the timer cannot
|
||||
* result in missing the deadline.
|
||||
*/
|
||||
#define HRT_INTERVAL_MIN 50
|
||||
#define HRT_INTERVAL_MAX 4000000000
|
||||
|
||||
/*
|
||||
* Scaling factor(s) for the free-running counter; convert an input
|
||||
* in counts to a time in microseconds.
|
||||
*/
|
||||
#define HRT_COUNTER_SCALE(_c) (_c)
|
||||
|
||||
/*
|
||||
* Timer register accessors
|
||||
*/
|
||||
#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
|
||||
|
||||
#define rTIMEHW REG(0x0) // Write to bits 63:32 of time always write timelw before timehw
|
||||
#define rTIMELW REG(0x4) // Write to bits 31:0 of time writes do not get copied to time until timehw is written
|
||||
#define rTIMEHR REG(0x8) // Read from bits 63:32 of time always read timelr before timehr
|
||||
#define rTIMELR REG(0xc) // Read from bits 31:0 of time
|
||||
#define rALARM0 REG(0x10) // Arm alarm 0, and configure the time it will fire
|
||||
#define rALARM1 REG(0x14) // Arm alarm 1, and configure the time it will fire
|
||||
#define rALARM2 REG(0x18) // Arm alarm 2, and configure the time it will fire
|
||||
#define rALARM3 REG(0x1c) // Arm alarm 3, and configure the time it will fire
|
||||
#define rARMED REG(0x20) // Indicates the armed/disarmed status of each alarm
|
||||
#define rTIMERAWH REG(0x24) // Raw read from bits 63:32 of time
|
||||
#define rTIMERAWL REG(0x28) // Raw read from bits 31:0 of time
|
||||
#define rDBGPAUSE REG(0x2c) // Set bits high to enable pause when the corresponding debug ports are active
|
||||
#define rPAUSE REG(0x30) // Set high to pause the timer
|
||||
#define rINTR REG(0x34) // Raw Interrupts
|
||||
#define rINTE REG(0x38) // Interrupt Enable
|
||||
#define rINTF REG(0x3c) // Interrupt Force
|
||||
#define rINTS REG(0x40) // Interrupt status after masking & forcing
|
||||
|
||||
/*
|
||||
* Specific registers and bits used by HRT sub-functions
|
||||
*/
|
||||
#if HRT_TIMER_CHANNEL == 1
|
||||
# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_0 // Timer alarm interrupt vector //
|
||||
# define HRT_ALARM_VALUE rALARM0 // Alarm register for HRT (similar to compare register for other MCUs) //
|
||||
# define HRT_ALARM_ENABLE (1 << 0) // Bit-0 for alarm 0 //
|
||||
#elif HRT_TIMER_CHANNEL == 2
|
||||
# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_1 // Timer alarm interrupt vector //
|
||||
# define HRT_ALARM_VALUE rALARM1 // Alarm register for HRT (similar to compare register for other MCUs) //
|
||||
# define HRT_ALARM_ENABLE (1 << 1) // Bit-1 for alarm 1 //
|
||||
#elif HRT_TIMER_CHANNEL == 3
|
||||
# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_2 // Timer alarm interrupt vector //
|
||||
# define HRT_ALARM_VALUE rALARM2 // Alarm register for HRT (similar to compare register for other MCUs) //
|
||||
# define HRT_ALARM_ENABLE (1 << 2) // Bit-2 for alarm 2 //
|
||||
#elif HRT_TIMER_CHANNEL == 4
|
||||
# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_3 // Timer alarm interrupt vector //
|
||||
# define HRT_ALARM_VALUE rALARM3 // Alarm register for HRT (similar to compare register for other MCUs) //
|
||||
# define HRT_ALARM_ENABLE (1 << 3) // Bit-3 for alarm 3 //
|
||||
#else
|
||||
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Queue of callout entries.
|
||||
*/
|
||||
static struct sq_queue_s callout_queue;
|
||||
|
||||
/* latency baseline (last compare value applied) */
|
||||
static uint64_t latency_baseline;
|
||||
|
||||
/* timer count at interrupt (for latency purposes) */
|
||||
static uint64_t latency_actual;
|
||||
|
||||
/* latency histogram */
|
||||
const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
|
||||
const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
|
||||
__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
|
||||
|
||||
/* timer-specific functions */
|
||||
static void hrt_tim_init(void);
|
||||
static int hrt_tim_isr(int irq, void *context, void *arg);
|
||||
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);
|
||||
|
||||
// For PPM, a PWM timer is not required to be used. This will be handled by activating
|
||||
// edge detection interrupts on the gpio. This way, the value of hrt can be recorded
|
||||
// when irq_handler is called and the PPM signal can be decoded. Also, there is no way
|
||||
// to check wheter the interrupt was caused by a rising edge or a falling edge. This can
|
||||
// be done by changing the interrupt type (rising/falling) every time the interrupt is
|
||||
// triggered.
|
||||
|
||||
/*
|
||||
* Specific registers and bits used by PPM sub-functions
|
||||
*/
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/*
|
||||
* 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;
|
||||
|
||||
#define PPM_DEBUG 0
|
||||
|
||||
#if PPM_DEBUG
|
||||
/* PPM edge history */
|
||||
__EXPORT uint16_t ppm_edge_history[32];
|
||||
unsigned ppm_edge_next;
|
||||
|
||||
/* PPM pulse history */
|
||||
__EXPORT uint16_t ppm_pulse_history[32];
|
||||
unsigned ppm_pulse_next;
|
||||
#endif
|
||||
|
||||
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
|
||||
|
||||
/** PPM decoder state machine */
|
||||
struct {
|
||||
uint16_t last_edge; /**< last capture time */
|
||||
uint16_t last_mark; /**< last significant edge */
|
||||
uint16_t frame_start; /**< the frame width */
|
||||
unsigned next_channel; /**< next channel index */
|
||||
enum {
|
||||
UNSYNCH = 0,
|
||||
ARM,
|
||||
ACTIVE,
|
||||
INACTIVE
|
||||
} phase;
|
||||
} ppm;
|
||||
|
||||
// Store the last detected edge type
|
||||
enum edgeType {
|
||||
falling = 0,
|
||||
rising,
|
||||
};
|
||||
enum edgeType lastEdge = falling;
|
||||
|
||||
// PPM specific functions
|
||||
static void hrt_ppm_decode(uint16_t counterVal);
|
||||
static int hrt_ppm_isr(int irq, void *context, void *arg);
|
||||
|
||||
#endif /* HRT_PPM_CHANNEL */
|
||||
|
||||
/**
|
||||
* Initialise the timer we are going to use.
|
||||
*/
|
||||
static void
|
||||
hrt_tim_init(void)
|
||||
{
|
||||
/* claim our interrupt vector */
|
||||
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL);
|
||||
|
||||
/* Enable alarm interrupt */
|
||||
rINTE = HRT_ALARM_ENABLE;
|
||||
|
||||
/* Enable vector interrupt */
|
||||
up_enable_irq(HRT_TIMER_VECTOR);
|
||||
|
||||
/* Set Initial capture a little ways off */
|
||||
HRT_ALARM_VALUE = rTIMERAWL + 1000;
|
||||
}
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/**
|
||||
* Handle the PPM decoder state machine.
|
||||
*/
|
||||
static void
|
||||
hrt_ppm_decode(uint16_t counterVal)
|
||||
{
|
||||
uint16_t count = counterVal;
|
||||
uint16_t width;
|
||||
uint16_t interval;
|
||||
unsigned i;
|
||||
|
||||
// If we miss an edge then we will miss a whole pulse.
|
||||
// So, frame discarding will be taken care of by the
|
||||
// width value.
|
||||
|
||||
/* 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 >= 32) {
|
||||
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 >= 32) {
|
||||
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 = hrt_absolute_time();
|
||||
|
||||
rINTR = rINTR; // ack the interrupts we just read
|
||||
|
||||
/* It is never a timer tick. It is always triggered by alarm. */
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle the compare interrupt by calling the callout dispatcher
|
||||
* and then re-scheduling the next deadline.
|
||||
*/
|
||||
static int
|
||||
hrt_ppm_isr(int irq, void *context, void *arg)
|
||||
{
|
||||
// Read lower 16 bits of hrt
|
||||
uint16_t counter = rTIMERAWL & 0xffff;
|
||||
|
||||
// Switch the next interrupt type
|
||||
px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL);
|
||||
lastEdge = !lastEdge;
|
||||
|
||||
hrt_ppm_decode(counter);
|
||||
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)
|
||||
{
|
||||
/* Taken from rp2040 datasheet pg. 558 */
|
||||
uint32_t hi = rTIMERAWH;
|
||||
uint32_t lo;
|
||||
|
||||
do {
|
||||
lo = rTIMERAWL;
|
||||
uint32_t next_hi = rTIMERAWH;
|
||||
|
||||
if (hi == next_hi) { break; }
|
||||
|
||||
hi = next_hi;
|
||||
} while (true);
|
||||
|
||||
return ((uint64_t) hi << 32) | lo;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
void hrt_store_absolute_time(volatile hrt_abstime *t)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
*t = hrt_absolute_time();
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the high-resolution timing module.
|
||||
*/
|
||||
void
|
||||
hrt_init(void)
|
||||
{
|
||||
sq_init(&callout_queue);
|
||||
hrt_tim_init();
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
// Set up edge detection interrupt on the PPM gpio.
|
||||
px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL);
|
||||
lastEdge = !lastEdge;
|
||||
|
||||
/* 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 uninitialised
|
||||
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 uninitialised entry->link but we don't do
|
||||
anything actually unsafe.
|
||||
*/
|
||||
if (entry->deadline != 0) {
|
||||
sq_rem(&entry->link, &callout_queue);
|
||||
}
|
||||
|
||||
entry->deadline = deadline;
|
||||
entry->period = interval;
|
||||
entry->callout = callout;
|
||||
entry->arg = arg;
|
||||
|
||||
hrt_call_enter(entry);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
/**
|
||||
* If this returns true, the call has been invoked and removed from the callout list.
|
||||
*
|
||||
* Always returns false for repeating callouts.
|
||||
*/
|
||||
bool
|
||||
hrt_called(struct hrt_call *entry)
|
||||
{
|
||||
return (entry->deadline == 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Remove the entry from the callout list.
|
||||
*/
|
||||
void
|
||||
hrt_cancel(struct hrt_call *entry)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
sq_rem(&entry->link, &callout_queue);
|
||||
entry->deadline = 0;
|
||||
|
||||
/* if this is a periodic call being removed by the callout, prevent it from
|
||||
* being re-entered when the callout returns.
|
||||
*/
|
||||
entry->period = 0;
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_call_enter(struct hrt_call *entry)
|
||||
{
|
||||
struct hrt_call *call, *next;
|
||||
|
||||
call = (struct hrt_call *)sq_peek(&callout_queue);
|
||||
|
||||
if ((call == NULL) || (entry->deadline < call->deadline)) {
|
||||
sq_addfirst(&entry->link, &callout_queue);
|
||||
hrtinfo("call enter at head, reschedule\n");
|
||||
/* we changed the next deadline, reschedule the timer event */
|
||||
hrt_call_reschedule();
|
||||
|
||||
} else {
|
||||
do {
|
||||
next = (struct hrt_call *)sq_next(&call->link);
|
||||
|
||||
if ((next == NULL) || (entry->deadline < next->deadline)) {
|
||||
hrtinfo("call enter after head\n");
|
||||
sq_addafter(&call->link, &entry->link, &callout_queue);
|
||||
break;
|
||||
}
|
||||
} while ((call = next) != NULL);
|
||||
}
|
||||
|
||||
hrtinfo("scheduled\n");
|
||||
}
|
||||
|
||||
static void
|
||||
hrt_call_invoke(void)
|
||||
{
|
||||
struct hrt_call *call;
|
||||
hrt_abstime deadline;
|
||||
|
||||
while (true) {
|
||||
/* get the current time */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
call = (struct hrt_call *)sq_peek(&callout_queue);
|
||||
|
||||
if (call == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (call->deadline > now) {
|
||||
break;
|
||||
}
|
||||
|
||||
sq_rem(&call->link, &callout_queue);
|
||||
hrtinfo("call pop\n");
|
||||
|
||||
/* save the intended deadline for periodic calls */
|
||||
deadline = call->deadline;
|
||||
|
||||
/* zero the deadline, as the call has occurred */
|
||||
call->deadline = 0;
|
||||
|
||||
/* invoke the callout (if there is one) */
|
||||
if (call->callout) {
|
||||
hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg);
|
||||
call->callout(call->arg);
|
||||
}
|
||||
|
||||
/* if the callout has a non-zero period, it has to be re-entered */
|
||||
if (call->period != 0) {
|
||||
// re-check call->deadline to allow for
|
||||
// callouts to re-schedule themselves
|
||||
// using hrt_call_delay()
|
||||
if (call->deadline <= now) {
|
||||
call->deadline = deadline + call->period;
|
||||
}
|
||||
|
||||
hrt_call_enter(call);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reschedule the next timer interrupt.
|
||||
*
|
||||
* This routine must be called with interrupts disabled.
|
||||
*/
|
||||
static void
|
||||
hrt_call_reschedule()
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
|
||||
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
|
||||
|
||||
/*
|
||||
* Determine what the next deadline will be.
|
||||
*
|
||||
* Note that we ensure that this will be within the counter
|
||||
* period, so that when we truncate all but the low 16 bits
|
||||
* the next time the compare matches it will be the deadline
|
||||
* we want.
|
||||
*
|
||||
* It is important for accurate timekeeping that the compare
|
||||
* interrupt fires sufficiently often that the base_time update in
|
||||
* hrt_absolute_time runs at least once per timer period.
|
||||
*/
|
||||
if (next != NULL) {
|
||||
hrtinfo("entry in queue\n");
|
||||
|
||||
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
|
||||
hrtinfo("pre-expired\n");
|
||||
/* set a minimal deadline so that we call ASAP */
|
||||
deadline = now + HRT_INTERVAL_MIN;
|
||||
|
||||
} else if (next->deadline < deadline) {
|
||||
hrtinfo("due soon\n");
|
||||
deadline = next->deadline;
|
||||
}
|
||||
}
|
||||
|
||||
hrtinfo("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
|
||||
|
||||
/* set the new compare value and remember it for latency tracking */
|
||||
HRT_ALARM_VALUE = (latency_baseline = deadline) & 0xffffffff;
|
||||
}
|
||||
|
||||
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,40 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#define SYSTEM_ADC_BASE RP2040_ADC_BASE
|
||||
|
||||
#include <px4_platform/adc.h>
|
||||
|
|
@ -0,0 +1,222 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <hardware/rp2040_memorymap.h>
|
||||
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
|
||||
|
||||
/*
|
||||
* Timers
|
||||
*/
|
||||
|
||||
namespace Timer
|
||||
{
|
||||
enum Timer {
|
||||
Timer0 = 1,
|
||||
Timer1,
|
||||
Timer2,
|
||||
Timer3,
|
||||
Timer4,
|
||||
Timer5,
|
||||
Timer6,
|
||||
Timer7,
|
||||
};
|
||||
enum Channel {
|
||||
ChannelA = 1,
|
||||
ChannelB,
|
||||
};
|
||||
struct TimerChannel {
|
||||
Timer timer;
|
||||
Channel channel;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
|
||||
{
|
||||
switch (timer) {
|
||||
case Timer::Timer0: return RP2040_PWM_BASE + 0x00;
|
||||
|
||||
case Timer::Timer1: return RP2040_PWM_BASE + 0x14;
|
||||
|
||||
case Timer::Timer2: return RP2040_PWM_BASE + 0x28;
|
||||
|
||||
case Timer::Timer3: return RP2040_PWM_BASE + 0x3c;
|
||||
|
||||
case Timer::Timer4: return RP2040_PWM_BASE + 0x50;
|
||||
|
||||
case Timer::Timer5: return RP2040_PWM_BASE + 0x64;
|
||||
|
||||
case Timer::Timer6: return RP2040_PWM_BASE + 0x78;
|
||||
|
||||
case Timer::Timer7: return RP2040_PWM_BASE + 0x8c;
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* GPIO
|
||||
*/
|
||||
|
||||
namespace GPIO
|
||||
{
|
||||
// RP2040 doesn't have PORTS
|
||||
enum Pin {
|
||||
Pin0 = 0,
|
||||
Pin1,
|
||||
Pin2,
|
||||
Pin3,
|
||||
Pin4,
|
||||
Pin5,
|
||||
Pin6,
|
||||
Pin7,
|
||||
Pin8,
|
||||
Pin9,
|
||||
Pin10,
|
||||
Pin11,
|
||||
Pin12,
|
||||
Pin13,
|
||||
Pin14,
|
||||
Pin15,
|
||||
Pin16,
|
||||
Pin17,
|
||||
Pin18,
|
||||
Pin19,
|
||||
Pin20,
|
||||
Pin21,
|
||||
Pin22,
|
||||
Pin23,
|
||||
Pin24,
|
||||
Pin25,
|
||||
Pin26,
|
||||
Pin27,
|
||||
Pin28,
|
||||
Pin29,
|
||||
};
|
||||
|
||||
struct GPIOPin {
|
||||
Pin pin;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
|
||||
{
|
||||
switch (pin) {
|
||||
case GPIO::Pin0: return 0;
|
||||
|
||||
case GPIO::Pin1: return 1;
|
||||
|
||||
case GPIO::Pin2: return 2;
|
||||
|
||||
case GPIO::Pin3: return 3;
|
||||
|
||||
case GPIO::Pin4: return 4;
|
||||
|
||||
case GPIO::Pin5: return 5;
|
||||
|
||||
case GPIO::Pin6: return 6;
|
||||
|
||||
case GPIO::Pin7: return 7;
|
||||
|
||||
case GPIO::Pin8: return 8;
|
||||
|
||||
case GPIO::Pin9: return 9;
|
||||
|
||||
case GPIO::Pin10: return 10;
|
||||
|
||||
case GPIO::Pin11: return 11;
|
||||
|
||||
case GPIO::Pin12: return 12;
|
||||
|
||||
case GPIO::Pin13: return 13;
|
||||
|
||||
case GPIO::Pin14: return 14;
|
||||
|
||||
case GPIO::Pin15: return 15;
|
||||
|
||||
case GPIO::Pin16: return 16;
|
||||
|
||||
case GPIO::Pin17: return 17;
|
||||
|
||||
case GPIO::Pin18: return 18;
|
||||
|
||||
case GPIO::Pin19: return 19;
|
||||
|
||||
case GPIO::Pin20: return 20;
|
||||
|
||||
case GPIO::Pin21: return 21;
|
||||
|
||||
case GPIO::Pin22: return 22;
|
||||
|
||||
case GPIO::Pin23: return 23;
|
||||
|
||||
case GPIO::Pin24: return 24;
|
||||
|
||||
case GPIO::Pin25: return 25;
|
||||
|
||||
case GPIO::Pin26: return 26;
|
||||
|
||||
case GPIO::Pin27: return 27;
|
||||
|
||||
case GPIO::Pin28: return 28;
|
||||
|
||||
case GPIO::Pin29: return 29;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace SPI
|
||||
{
|
||||
enum class Bus {
|
||||
SPI0 = 1,
|
||||
SPI1,
|
||||
};
|
||||
|
||||
using CS = GPIO::GPIOPin; ///< chip-select pin
|
||||
using DRDY = GPIO::GPIOPin; ///< data ready pin
|
||||
|
||||
struct bus_device_external_cfg_t {
|
||||
CS cs_gpio;
|
||||
DRDY drdy_gpio;
|
||||
};
|
||||
|
||||
} // namespace SPI
|
|
@ -0,0 +1,54 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/i2c.h>
|
||||
|
||||
|
||||
static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
|
||||
{
|
||||
px4_i2c_bus_t ret{};
|
||||
ret.bus = bus;
|
||||
ret.is_external = false;
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
|
||||
{
|
||||
px4_i2c_bus_t ret{};
|
||||
ret.bus = bus;
|
||||
ret.is_external = true;
|
||||
return ret;
|
||||
}
|
|
@ -0,0 +1,163 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file io_timer.h
|
||||
*/
|
||||
#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 */
|
||||
#ifdef BOARD_NUM_IO_TIMERS
|
||||
#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS
|
||||
#else
|
||||
#define MAX_IO_TIMERS 8
|
||||
#endif
|
||||
#if DIRECT_PWM_OUTPUT_CHANNELS > 8
|
||||
#define MAX_TIMER_IO_CHANNELS DIRECT_PWM_OUTPUT_CHANNELS
|
||||
#else
|
||||
#define MAX_TIMER_IO_CHANNELS 16
|
||||
#endif
|
||||
|
||||
#define MAX_LED_TIMERS 2
|
||||
#define MAX_TIMER_LED_CHANNELS 4
|
||||
|
||||
#define IO_TIMER_ALL_MODES_CHANNELS 0
|
||||
|
||||
/* TIM_DMA_Base_address TIM DMA Base Address */
|
||||
#define TIM_DMABASE_CCR1 0x0000000DU
|
||||
#define TIM_DMABASE_CCR2 0x0000000EU
|
||||
#define TIM_DMABASE_CCR3 0x0000000FU
|
||||
#define TIM_DMABASE_CCR4 0x00000010U
|
||||
|
||||
typedef enum io_timer_channel_mode_t {
|
||||
IOTimerChanMode_NotUsed = 0,
|
||||
IOTimerChanMode_PWMOut = 1,
|
||||
IOTimerChanMode_PWMIn = 2,
|
||||
IOTimerChanMode_Capture = 3,
|
||||
IOTimerChanMode_OneShot = 4,
|
||||
IOTimerChanMode_Trigger = 5,
|
||||
IOTimerChanMode_Dshot = 6,
|
||||
IOTimerChanModeSize
|
||||
} io_timer_channel_mode_t;
|
||||
|
||||
typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
|
||||
|
||||
/* array of timers dedicated to PWM in and out and capture use
|
||||
*** Note that the clock_freq is set to the source in the clock tree that
|
||||
*** feeds this specific timer. This can differs by Timer!
|
||||
*** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz
|
||||
*** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz
|
||||
*** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz
|
||||
*** the resulting PSC will be one and the timer will count at it's clock frequency.
|
||||
*/
|
||||
typedef struct io_timers_t {
|
||||
uint32_t base;
|
||||
// uint32_t clock_register; // Not required for rp2040
|
||||
// uint32_t clock_bit;
|
||||
// uint32_t vectorno;
|
||||
// dshot_conf_t dshot;
|
||||
} io_timers_t;
|
||||
|
||||
typedef struct io_timers_channel_mapping_element_t {
|
||||
uint32_t first_channel_index;
|
||||
uint32_t channel_count;
|
||||
} io_timers_channel_mapping_element_t;
|
||||
|
||||
/* mapping for each io_timers to timer_io_channels */
|
||||
typedef struct io_timers_channel_mapping_t {
|
||||
io_timers_channel_mapping_element_t element[MAX_IO_TIMERS];
|
||||
} io_timers_channel_mapping_t;
|
||||
|
||||
/* array of channels in logical order */
|
||||
typedef struct timer_io_channels_t {
|
||||
uint32_t gpio_out;
|
||||
uint32_t gpio_in;
|
||||
uint8_t timer_index;
|
||||
uint8_t timer_channel;
|
||||
} timer_io_channels_t;
|
||||
|
||||
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
|
||||
const timer_io_channels_t *chan,
|
||||
hrt_abstime isrs_time, uint16_t isrs_rcnt);
|
||||
|
||||
|
||||
/* supplied by board-specific code */
|
||||
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
|
||||
__EXPORT extern const io_timers_channel_mapping_t io_timers_channel_mapping;
|
||||
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
|
||||
__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
|
||||
|
||||
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
|
||||
|
||||
__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);
|
||||
__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
|
||||
|
||||
__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
|
||||
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as GPIO output.
|
||||
* 0 is returned if the channel is not valid.
|
||||
*/
|
||||
__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel);
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as PWM input.
|
||||
* 0 is returned if the channel is not valid.
|
||||
*/
|
||||
__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel);
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,157 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/io_timer_init.h>
|
||||
|
||||
static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
|
||||
Timer::TimerChannel timer, GPIO::GPIOPin pin)
|
||||
{
|
||||
timer_io_channels_t ret{};
|
||||
|
||||
uint32_t gpio_af = 0;
|
||||
|
||||
switch (timer.channel) {
|
||||
case Timer::ChannelA:
|
||||
if (!(pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) {
|
||||
gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case Timer::ChannelB:
|
||||
if ((pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) {
|
||||
gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
ret.gpio_in = gpio_af;
|
||||
ret.gpio_out = gpio_af;
|
||||
|
||||
ret.timer_channel = timer.channel;
|
||||
|
||||
// find timer index
|
||||
ret.timer_index = 0xff;
|
||||
const uint32_t timer_base = timerBaseRegister(timer.timer);
|
||||
|
||||
for (int i = 0; i < MAX_IO_TIMERS; ++i) {
|
||||
if (io_timers_conf[i].base == timer_base) {
|
||||
ret.timer_index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr_assert(ret.timer_index != 0xff, "Timer not found");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer)
|
||||
{
|
||||
bool nuttx_config_timer_enabled = false;
|
||||
io_timers_t ret{};
|
||||
|
||||
switch (timer) {
|
||||
case Timer::Timer0:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH0 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer1:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH1 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer2:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH2 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer3:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH3 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer4:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH4 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer5:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH5 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer6:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH6 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case Timer::Timer7:
|
||||
ret.base = timerBaseRegister(timer);
|
||||
#ifdef CONFIG_RP2040_PWM_CH7 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
|
||||
nuttx_config_timer_enabled = true;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
// This is not strictly required, but for consistency let's make sure NuttX timers are disabled
|
||||
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (KINETIS_FTMx)");
|
||||
|
||||
return ret;
|
||||
}
|
|
@ -0,0 +1,107 @@
|
|||
#pragma once
|
||||
|
||||
#include <px4_platform/micro_hal.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#include <rp2040_spi.h>
|
||||
#include <rp2040_i2c.h>
|
||||
#include <rp2040_gpio.h>
|
||||
|
||||
// RP2040 doesn't have a bbsram. Following two defines are copied from nxp/k66.
|
||||
// This will remove the errors of undefined PX4_BBSRAM_SIZE when logger module is activated.
|
||||
// Fixme: using ??
|
||||
#define PX4_BBSRAM_SIZE 2048
|
||||
#define PX4_BBSRAM_GETDESC_IOCTL 0
|
||||
|
||||
// RP2040 doesn't really have a cpu register with unique id.
|
||||
// However, there is a function in pico-sdk which can provide
|
||||
// a device unique id from its flash which is 64 bits in length.
|
||||
// For now, a common device id will be used for all RP2040 based devices.
|
||||
// Take a look at board_identity.c file in version folder.
|
||||
|
||||
#define PX4_CPU_UUID_BYTE_LENGTH 12
|
||||
#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
|
||||
|
||||
/* By not defining PX4_CPU_UUID_CORRECT_CORRELATION the following maintains the legacy incorrect order
|
||||
* used for selection of significant digits of the UUID in the PX4 code base.
|
||||
* This is done to avoid the ripple effects changing the IDs used on STM32 base platforms
|
||||
*/
|
||||
#if defined(PX4_CPU_UUID_CORRECT_CORRELATION)
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_H 0 /* Least significant digits change the most */
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Middle significant digits */
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_L 2 /* Most significant digits change the least */
|
||||
#else
|
||||
/* Legacy incorrect ordering */
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_H 2 /* Most significant digits change the least */
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Middle significant digits */
|
||||
# define PX4_CPU_UUID_WORD32_UNIQUE_L 0 /* Least significant digits change the most */
|
||||
#endif
|
||||
|
||||
/* 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 px4_savepanic(fileno, context, length) (0) // Turn off px4_savepanic for rp2040 as it is not implemented in nuttx
|
||||
|
||||
#define PX4_BUS_OFFSET 1 /* RP2040 buses are 0 based and adjustment is needed */
|
||||
#define px4_spibus_initialize(bus_num_1based) rp2040_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
|
||||
|
||||
#define px4_i2cbus_initialize(bus_num_1based) rp2040_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
|
||||
#define px4_i2cbus_uninitialize(pdev) rp2040_i2cbus_uninitialize(pdev)
|
||||
|
||||
// This part of the code is specific to rp2040.
|
||||
// RP2040 does not have the gpio configuration process similar to stm or tiva devices.
|
||||
// There are multiple different registers which are required to be configured based on the function selection.
|
||||
// However, only five values are required for the most part: Pin number, Pull up/down, direction, set/clear and function
|
||||
// The pinset below can be defined using a 16-bit value where,
|
||||
// bits Function
|
||||
// 0-4 GPIO number. 0-29 is valid.
|
||||
// 5 Pull up
|
||||
// 6 Pull down
|
||||
// 7 Direction
|
||||
// 8 Set/clear
|
||||
// 9-13 GPIO function select
|
||||
// 14-15 Unused
|
||||
#define GPIO_PU (1 << 5) // Pull-up resistor
|
||||
#define GPIO_PD (1 << 6) // Pull-down resistor
|
||||
#define GPIO_OUT (1 << 7) // Output enable
|
||||
#define GPIO_SET (1 << 8) // Output set
|
||||
#define GPIO_FUN(func) (func << 9) // Function select
|
||||
|
||||
#define GPIO_NUM_MASK 0x1f
|
||||
#define GPIO_PU_MASK 0x20 // GPIO PAD register mask
|
||||
#define GPIO_PD_MASK 0x40 // GPIO pin number mask
|
||||
#define GPIO_OUT_MASK 0x80 // GPIO pin function mask
|
||||
#define GPIO_SET_MASK 0x100 // GPIO pin function mask
|
||||
#define GPIO_FUN_MASK 0x3E00 // GPIO output enable mask
|
||||
|
||||
int rp2040_gpioconfig(uint32_t pinset);
|
||||
int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
|
||||
|
||||
#define px4_arch_configgpio(pinset) rp2040_gpioconfig(pinset) // Defined in io_pins/rp2040_pinset.c
|
||||
#define px4_arch_unconfiggpio(pinset) rp2040_gpio_init(pinset & GPIO_NUM_MASK) // Reset the pin as input SIO
|
||||
#define px4_arch_gpioread(pinset) rp2040_gpio_get(pinset & GPIO_NUM_MASK) // Use gpio_get
|
||||
#define px4_arch_gpiowrite(pinset, value) rp2040_gpio_put(pinset & GPIO_NUM_MASK, value) // Use gpio_put
|
||||
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) rp2040_setgpioevent(pinset,r,f,e,fp,a) // Defined in io_pins/rp2040_pinset.c
|
||||
|
||||
// Following are quick defines to be used with the functions defined above
|
||||
// These defines create a bit-mask which is supposed to be used in the
|
||||
// functions defined above to set up gpios correctly.
|
||||
#define PX4_MAKE_GPIO_INPUT(gpio) (gpio | GPIO_PU | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
|
||||
#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (gpio | GPIO_OUT | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
|
||||
#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (gpio | GPIO_OUT | GPIO_SET | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
|
||||
|
||||
#define PX4_GPIO_PIN_OFF(pinset) ((pinset & GPIO_NUM_MASK) | GPIO_FUN(RP2040_GPIO_FUNC_SIO) | GPIO_PD)
|
||||
|
||||
#define px4_cache_aligned_data()
|
||||
#define px4_cache_aligned_alloc malloc
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,122 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/spi.h>
|
||||
#include <px4_arch/micro_hal.h>
|
||||
|
||||
static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
|
||||
{
|
||||
px4_spi_bus_device_t ret{};
|
||||
ret.cs_gpio = getGPIOPin(cs_gpio.pin) | (GPIO_OUT | GPIO_SET);
|
||||
ret.drdy_gpio = getGPIOPin(drdy_gpio.pin) | GPIO_PU; // GPIO_PU taken from kinetis
|
||||
|
||||
if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external)
|
||||
ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid);
|
||||
|
||||
} else { // it's a NuttX device (e.g. SPIDEV_FLASH(0))
|
||||
ret.devid = devid;
|
||||
}
|
||||
|
||||
ret.devtype_driver = PX4_SPI_DEV_ID(devid);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices,
|
||||
GPIO::GPIOPin power_enable = {})
|
||||
{
|
||||
px4_spi_bus_t ret{};
|
||||
ret.requires_locking = false;
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
ret.devices[i] = devices.devices[i];
|
||||
|
||||
|
||||
if (ret.devices[i].cs_gpio != 0) {
|
||||
if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
|
||||
int same_devices_count = 0;
|
||||
|
||||
for (int j = 0; j < i; ++j) {
|
||||
if (ret.devices[j].cs_gpio != 0) {
|
||||
same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
|
||||
}
|
||||
}
|
||||
|
||||
// increment the 2. LSB byte to allow multiple devices of the same type
|
||||
ret.devices[i].devid |= same_devices_count << 8;
|
||||
|
||||
} else {
|
||||
// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
|
||||
ret.requires_locking = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ret.bus = (int)bus;
|
||||
ret.is_external = false;
|
||||
|
||||
ret.power_enable_gpio = getGPIOPin(power_enable.pin) | GPIO_OUT;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments
|
||||
struct bus_device_external_cfg_array_t {
|
||||
SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES];
|
||||
};
|
||||
|
||||
static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices)
|
||||
{
|
||||
px4_spi_bus_t ret{};
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio);
|
||||
}
|
||||
|
||||
ret.bus = (int)bus;
|
||||
ret.is_external = true;
|
||||
ret.requires_locking = false; // external buses are never accessed by NuttX drivers
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
|
||||
{
|
||||
SPI::bus_device_external_cfg_t ret{};
|
||||
ret.cs_gpio = cs_gpio;
|
||||
ret.drdy_gpio = drdy_gpio;
|
||||
return ret;
|
||||
}
|
||||
|
||||
constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]);
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_io_pins
|
||||
io_timer.c
|
||||
pwm_servo.c
|
||||
# pwm_trigger.c
|
||||
# input_capture.c
|
||||
rp2040_pinset.c
|
||||
)
|
||||
|
||||
target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
|
@ -0,0 +1,768 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file io_timer.c
|
||||
*
|
||||
* Servo driver supporting PWM servos connected to RP2040 PWM 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>
|
||||
|
||||
// RP2040 PWM block has 8 slices (timers) and each has 2 independent outputs (channels) A and B.
|
||||
// All the channels can output pwm. However, only channel B can be used for input where the timer
|
||||
// will be working in edge sensitive mode or level sensitive mode. Be careful when choosing the
|
||||
// timer and channel for input capture. Each timer has an independent 8.4 fractional divider.
|
||||
// Each timer also has double buffered wrap (rTOP) and level (rCC) registers so the value can
|
||||
// change while PWM is running.
|
||||
|
||||
#if !defined(BOARD_PWM_FREQ)
|
||||
#define BOARD_PWM_FREQ 1000000
|
||||
#endif
|
||||
|
||||
#if !defined(BOARD_ONESHOT_FREQ)
|
||||
#define BOARD_ONESHOT_FREQ 8000000
|
||||
#endif
|
||||
|
||||
#define TIM_SRC_CLOCK_FREQ 125000000
|
||||
|
||||
#define MAX_CHANNELS_PER_TIMER 2
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
|
||||
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
|
||||
|
||||
// Register offsets
|
||||
#define RP2040_PWM_CSR_OFFSET 0x00 // Control and status register
|
||||
#define RP2040_PWM_DIV_OFFSET 0x04 // Clock divide register
|
||||
#define RP2040_PWM_CTR_OFFSET 0x08 // Direct access to the PWM counter
|
||||
#define RP2040_PWM_CCR_OFFSET 0x0c // Counter compare values
|
||||
#define RP2040_PWM_TOP_OFFSET 0x10 // Counter wrap value
|
||||
#define RP2040_PWM_EN_OFFSET 0xa0 // This register aliases the CSR_EN bits for all channels
|
||||
#define RP2040_PWM_INTR_OFFSET 0xa4 // Raw Interrupts
|
||||
#define RP2040_PWM_INTE_OFFSET 0xa8 // Interrupt Enable
|
||||
#define RP2040_PWM_INTF_OFFSET 0xac // Interrupt Force
|
||||
#define RP2040_PWM_INTS_OFFSET 0xb0 // Interrupt status after masking & forcing
|
||||
|
||||
/* Timer register accessors */
|
||||
#define rCSR(_tmr) REG(_tmr,RP2040_PWM_CSR_OFFSET)
|
||||
#define rDIV(_tmr) REG(_tmr,RP2040_PWM_DIV_OFFSET)
|
||||
#define rCTR(_tmr) REG(_tmr,RP2040_PWM_CTR_OFFSET)
|
||||
#define rCCR(_tmr) REG(_tmr,RP2040_PWM_CCR_OFFSET)
|
||||
#define rTOP(_tmr) REG(_tmr,RP2040_PWM_TOP_OFFSET)
|
||||
#define rEN _REG32(RP2040_PWM_BASE,RP2040_PWM_EN_OFFSET)
|
||||
#define rINTR _REG32(RP2040_PWM_BASE,RP2040_PWM_INTR_OFFSET)
|
||||
#define rINTE _REG32(RP2040_PWM_BASE,RP2040_PWM_INTE_OFFSET)
|
||||
#define rINTF _REG32(RP2040_PWM_BASE,RP2040_PWM_INTF_OFFSET)
|
||||
#define rINTS _REG32(RP2040_PWM_BASE,RP2040_PWM_INTS_OFFSET)
|
||||
|
||||
// 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; // Used to trace whether the timer is initialized or not
|
||||
|
||||
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(int irq, void *context, void *arg)//uint16_t timer_index)
|
||||
{
|
||||
/* Read the count at the time of the interrupt */
|
||||
|
||||
// uint16_t count = rCNT(timer_index);
|
||||
|
||||
// /* Read the HRT at the time of the interrupt */
|
||||
|
||||
// hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// const io_timers_t *tmr = &io_timers[timer_index];
|
||||
|
||||
// /* What is pending */
|
||||
|
||||
// uint32_t statusr = rSTATUS(timer_index);
|
||||
|
||||
// /* Acknowledge all that are pending */
|
||||
|
||||
// rSTATUS(timer_index) = 0;
|
||||
|
||||
// /* Iterate over the timer_io_channels table */
|
||||
|
||||
// uint32_t first_channel_index = io_timers_channel_mapping.element[timer_index].first_channel_index;
|
||||
// uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer_index].channel_count;
|
||||
|
||||
// for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
|
||||
|
||||
// uint16_t chan = 1 << chan_index;
|
||||
|
||||
// if (statusr & chan) {
|
||||
|
||||
// io_timer_channel_stats[chan_index].isr_cout++;
|
||||
|
||||
// /* Call the client to read the rCnV etc and clear the CHnF */
|
||||
|
||||
// if (channel_handlers[chan_index].callback) {
|
||||
// channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr,
|
||||
// chan_index, &timer_io_channels[chan_index],
|
||||
// now, count, _REG32(tmr->base, KINETIS_FTM_CV_OFFSET(chan_index)));
|
||||
// }
|
||||
// }
|
||||
|
||||
// /* Did it set again during call out ?*/
|
||||
|
||||
// if (rSTATUS(timer_index) & chan) {
|
||||
|
||||
// /* Error we has a second edge before we serviced the fist */
|
||||
|
||||
// io_timer_channel_stats[chan_index].overflows++;
|
||||
// }
|
||||
// }
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline int validate_timer_index(unsigned timer)
|
||||
{
|
||||
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
|
||||
}
|
||||
|
||||
static inline int is_timer_uninitalized(unsigned timer)
|
||||
{
|
||||
int rv = 0;
|
||||
|
||||
if (once & 1 << timer) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static inline void set_timer_initalized(unsigned timer)
|
||||
{
|
||||
once |= 1 << timer;
|
||||
}
|
||||
|
||||
static inline void set_timer_deinitalized(unsigned timer)
|
||||
{
|
||||
once &= ~(1 << timer);
|
||||
}
|
||||
|
||||
static inline int channels_timer(unsigned channel)
|
||||
{
|
||||
return timer_io_channels[channel].timer_index;
|
||||
}
|
||||
|
||||
static uint32_t get_timer_channels(unsigned timer)
|
||||
{
|
||||
uint32_t channels = 0;
|
||||
static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
|
||||
|
||||
if (validate_timer_index(timer) < 0) {
|
||||
return channels;
|
||||
|
||||
} else {
|
||||
if (channels_cache[timer] == 0) {
|
||||
|
||||
/* Gather the channel bits that belong to the timer */
|
||||
|
||||
uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
|
||||
uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
|
||||
|
||||
for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
|
||||
channels |= 1 << chan_index;
|
||||
}
|
||||
|
||||
/* cache them */
|
||||
|
||||
channels_cache[timer] = channels;
|
||||
}
|
||||
}
|
||||
|
||||
return channels_cache[timer];
|
||||
}
|
||||
|
||||
static inline int is_channels_timer_uninitalized(unsigned channel)
|
||||
{
|
||||
return is_timer_uninitalized(channels_timer(channel));
|
||||
}
|
||||
|
||||
int io_timer_is_channel_free(unsigned channel)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_validate_channel_index(unsigned channel)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) {
|
||||
|
||||
unsigned timer = timer_io_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
|
||||
if ((io_timers[timer].base != 0) &&
|
||||
(timer_io_channels[channel].gpio_out != 0) &&
|
||||
(timer_io_channels[channel].gpio_in != 0)) {
|
||||
rv = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
uint32_t io_timer_channel_get_gpio_output(unsigned channel)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return timer_io_channels[channel].gpio_out;
|
||||
}
|
||||
|
||||
uint32_t io_timer_channel_get_as_pwm_input(unsigned channel)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return timer_io_channels[channel].gpio_in;
|
||||
}
|
||||
|
||||
|
||||
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
|
||||
{
|
||||
if (mode < IOTimerChanModeSize) {
|
||||
return channel_allocations[mode];
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_get_channel_mode(unsigned channel)
|
||||
{
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
|
||||
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
|
||||
if (bit & channel_allocations[mode]) {
|
||||
return mode;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode,
|
||||
io_timer_channel_mode_t new_mode)
|
||||
{
|
||||
/* If caller mode is not based on current setting adjust it */
|
||||
|
||||
if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) {
|
||||
mode = IOTimerChanMode_NotUsed;
|
||||
}
|
||||
|
||||
/* Remove old set of channels from original */
|
||||
|
||||
channel_allocations[mode] &= ~channels;
|
||||
|
||||
/* Will this change ?*/
|
||||
|
||||
uint32_t before = channel_allocations[new_mode] & channels;
|
||||
|
||||
/* add in the new set */
|
||||
|
||||
channel_allocations[new_mode] |= channels;
|
||||
|
||||
/* Indicate a mode change */
|
||||
|
||||
return before ^ channels;
|
||||
}
|
||||
|
||||
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = io_timer_is_channel_free(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
|
||||
channel_allocations[mode] |= bit;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
static inline int free_channel_resource(unsigned channel)
|
||||
{
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[mode] &= ~bit;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
|
||||
}
|
||||
|
||||
return mode;
|
||||
}
|
||||
|
||||
int io_timer_free_channel(unsigned channel)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_set_enable(false, mode, 1 << channel);
|
||||
free_channel_resource(channel);
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (mode != IOTimerChanMode_NotUsed) {
|
||||
rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
rv = allocate_channel_resource(channel, mode);
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
// RP2040 has a buffer for rTOP, so there shouldn't be any need to turn the timer off to change rTOP value
|
||||
rTOP(timer) = (BOARD_PWM_FREQ / rate) - 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline uint32_t freq2div(uint32_t freq)
|
||||
{
|
||||
return (TIM_SRC_CLOCK_FREQ << 4) / freq;
|
||||
}
|
||||
|
||||
static inline void io_timer_set_oneshot_mode(unsigned timer)
|
||||
{
|
||||
/* Ideally, we would want per channel One pulse mode in HW
|
||||
* Alas OPE stops the Timer not the channel
|
||||
* todo:We can do this in an ISR later
|
||||
* But since we do not have that
|
||||
* We try to get the longest rate we can.
|
||||
* On 16 bit timers this is 8.1 Ms.
|
||||
*/
|
||||
|
||||
rTOP(timer) = 0xffff;
|
||||
rDIV(timer) = freq2div(BOARD_ONESHOT_FREQ);
|
||||
}
|
||||
|
||||
static inline void io_timer_set_PWM_mode(unsigned timer)
|
||||
{
|
||||
rDIV(timer) = freq2div(BOARD_PWM_FREQ);
|
||||
}
|
||||
|
||||
void io_timer_trigger(void)
|
||||
{
|
||||
// This function is probably not important for RP2040 as the buffered registers are updated automatically on the timer wrap.
|
||||
|
||||
// int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);
|
||||
// uint32_t action_cache[MAX_IO_TIMERS] = {0};
|
||||
// int actions = 0;
|
||||
|
||||
// /* Pre-calculate the list of timers to Trigger */
|
||||
|
||||
// for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
|
||||
// if (validate_timer_index(timer) == 0) {
|
||||
// int channels = get_timer_channels(timer);
|
||||
|
||||
// if (oneshots & channels) {
|
||||
// action_cache[actions++] = io_timers[timer].base;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// /* Now do them all wit the shortest delay in between */
|
||||
|
||||
// irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
// for (actions = 0; actions < MAX_IO_TIMERS && action_cache[actions] != 0; actions++) {
|
||||
// _REG32(action_cache[actions], KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
|
||||
// }
|
||||
|
||||
// px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
int io_timer_init_timer(unsigned timer)
|
||||
{
|
||||
/* Do this only once per timer */
|
||||
|
||||
int rv = is_timer_uninitalized(timer);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
set_timer_initalized(timer);
|
||||
|
||||
/* disable and configure the timer */
|
||||
|
||||
rCSR(timer) = 0;
|
||||
rCTR(timer) = 0;
|
||||
|
||||
/* enable the timer */
|
||||
|
||||
io_timer_set_PWM_mode(timer);
|
||||
|
||||
/*
|
||||
* Note we do the Standard PWM Out init here
|
||||
* default to updating at 50Hz
|
||||
*/
|
||||
|
||||
timer_set_rate(timer, 50);
|
||||
|
||||
/*
|
||||
* Note that the timer is left disabled with IRQ subs installed
|
||||
* and active but DEIR bits are not set.
|
||||
*/
|
||||
xcpt_t handler = io_timer_handler;
|
||||
|
||||
if (handler) {
|
||||
irq_attach(RP2040_PWM_IRQ_WRAP, handler, NULL);
|
||||
up_enable_irq(RP2040_PWM_IRQ_WRAP);
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
int io_timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
int rv = EBUSY;
|
||||
|
||||
/* Get the channel bits that belong to the timer */
|
||||
|
||||
uint32_t channels = get_timer_channels(timer);
|
||||
|
||||
/* Check that all channels are either in PWM or Oneshot */
|
||||
|
||||
if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
|
||||
channel_allocations[IOTimerChanMode_OneShot] |
|
||||
channel_allocations[IOTimerChanMode_NotUsed])) ==
|
||||
channels) {
|
||||
|
||||
/* Change only a timer that is owned by pwm or one shot */
|
||||
|
||||
/* Request to use OneShot ?*/
|
||||
|
||||
if (rate == 0) {
|
||||
|
||||
/* Request to use OneShot
|
||||
*
|
||||
* We are here because ALL these channels were either PWM or Oneshot
|
||||
* Now they need to be Oneshot
|
||||
*/
|
||||
|
||||
/* Did the allocation change */
|
||||
if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
|
||||
io_timer_set_oneshot_mode(timer);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* Request to use PWM
|
||||
*
|
||||
* We are here because ALL these channels were either PWM or Oneshot
|
||||
* Now they need to be PWM
|
||||
*/
|
||||
|
||||
if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
|
||||
io_timer_set_PWM_mode(timer);
|
||||
}
|
||||
|
||||
timer_set_rate(timer, rate);
|
||||
}
|
||||
|
||||
rv = OK;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context)
|
||||
{
|
||||
// RP2040 doesn't have any specific configuration for each channel except the CCR value.
|
||||
// And RP2040 doesn't really have the hardware capability for input capture.
|
||||
|
||||
/* figure out the GPIO config first */
|
||||
switch (mode) {
|
||||
|
||||
case IOTimerChanMode_OneShot:
|
||||
case IOTimerChanMode_PWMOut:
|
||||
case IOTimerChanMode_Trigger:
|
||||
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) {
|
||||
|
||||
/* Blindly try to initialize the timer - it will only do it once */
|
||||
|
||||
io_timer_init_timer(channels_timer(channel));
|
||||
|
||||
/* configure the channel */ // Nothing to configure here really.
|
||||
|
||||
// uint32_t chan = timer_io_channels[channel].timer_channel - 1;
|
||||
|
||||
channel_handlers[channel].callback = channel_handler;
|
||||
channel_handlers[channel].context = context;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
|
||||
{
|
||||
typedef struct action_cache_rp_t {
|
||||
// uint32_t cnsc_offset;
|
||||
// uint32_t cnsc_value;
|
||||
uint32_t gpio;
|
||||
} action_cache_rp_t;
|
||||
struct action_cache_t {
|
||||
uint32_t base;
|
||||
uint32_t index;
|
||||
action_cache_rp_t cnsc[MAX_CHANNELS_PER_TIMER];
|
||||
} action_cache[MAX_IO_TIMERS];
|
||||
|
||||
memset(action_cache, 0, sizeof(action_cache));
|
||||
|
||||
// uint32_t bits = state ? CnSC_PWMOUT_INIT : 0;
|
||||
|
||||
switch (mode) {
|
||||
case IOTimerChanMode_NotUsed:
|
||||
case IOTimerChanMode_PWMOut:
|
||||
case IOTimerChanMode_OneShot:
|
||||
case IOTimerChanMode_Trigger:
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* Was the request for all channels in this mode ?*/
|
||||
|
||||
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
|
||||
|
||||
/* Yes - we provide them */
|
||||
|
||||
masks = channel_allocations[mode];
|
||||
|
||||
} else {
|
||||
|
||||
/* No - caller provided mask */
|
||||
|
||||
/* Only allow the channels in that mode to be affected */
|
||||
|
||||
masks &= channel_allocations[mode];
|
||||
|
||||
}
|
||||
|
||||
/* Pre calculate all the changes */
|
||||
|
||||
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
|
||||
if (masks & (1 << chan_index)) {
|
||||
masks &= ~(1 << chan_index);
|
||||
|
||||
if (io_timer_validate_channel_index(chan_index) == 0) {
|
||||
// uint32_t chan = timer_io_channels[chan_index].timer_channel - 1;
|
||||
uint32_t timer = channels_timer(chan_index);
|
||||
action_cache[timer].base = io_timers[timer].base;
|
||||
// action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + KINETIS_FTM_CSC_OFFSET(chan);
|
||||
// action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits;
|
||||
|
||||
if ((state &&
|
||||
(mode == IOTimerChanMode_PWMOut ||
|
||||
mode == IOTimerChanMode_OneShot ||
|
||||
mode == IOTimerChanMode_Trigger))) {
|
||||
action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out;
|
||||
}
|
||||
|
||||
action_cache[timer].index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
|
||||
for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
|
||||
// uint32_t any_on = 0;
|
||||
|
||||
if (action_cache[actions].base != 0) {
|
||||
for (unsigned int index = 0; index < action_cache[actions].index; index++) {
|
||||
|
||||
if (action_cache[actions].cnsc[index].gpio) {
|
||||
px4_arch_configgpio(action_cache[actions].cnsc[index].gpio);
|
||||
}
|
||||
|
||||
// _REG(action_cache[actions].cnsc[index].cnsc_offset) = action_cache[actions].cnsc[index].cnsc_value;
|
||||
// any_on |= action_cache[actions].cnsc[index].cnsc_value;
|
||||
}
|
||||
|
||||
/* Any On ?*/
|
||||
|
||||
/* Assume not */
|
||||
// uint32_t regval = _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET);
|
||||
// regval &= ~(FTM_SC_CLKS_MASK);
|
||||
|
||||
// if (any_on != 0) {
|
||||
|
||||
// /* force an update to preload all registers */
|
||||
// _REG32(action_cache[actions].base, KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
|
||||
|
||||
// /* arm requires the timer be enabled */
|
||||
// regval |= (FTM_SC_CLKS_EXTCLK);
|
||||
// }
|
||||
|
||||
_REG32(action_cache[actions].base, RP2040_PWM_CSR_OFFSET) |= state;
|
||||
}
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_set_ccr(unsigned channel, uint16_t value)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
int mode = io_timer_get_channel_mode(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if ((mode != IOTimerChanMode_PWMOut) &&
|
||||
(mode != IOTimerChanMode_OneShot) &&
|
||||
(mode != IOTimerChanMode_Trigger)) {
|
||||
|
||||
rv = -EIO;
|
||||
|
||||
} else {
|
||||
|
||||
/* configure the channel */
|
||||
int regVal = rCCR(channels_timer(channel));
|
||||
regVal &= ~(0xffff << (timer_io_channels[channel].timer_channel - 1) * 16);
|
||||
regVal |= value << (timer_io_channels[channel].timer_channel - 1) * 16;
|
||||
rCCR(channels_timer(channel)) = regVal;
|
||||
}
|
||||
}
|
||||
|
||||
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 = rCCR(channels_timer(channel)) >> (timer_io_channels[channel].timer_channel - 1) * 16;
|
||||
}
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
uint32_t io_timer_get_group(unsigned timer)
|
||||
{
|
||||
return get_timer_channels(timer);
|
||||
|
||||
}
|
|
@ -0,0 +1,158 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file drv_pwm_servo.c
|
||||
*
|
||||
* Servo driver supporting PWM servos connected to RP2040 PWM blocks.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/irq.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <time.h>
|
||||
#include <queue.h>
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include <px4_arch/io_timer.h>
|
||||
|
||||
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(uint32_t channel_mask)
|
||||
{
|
||||
/* disable the timers */
|
||||
up_pwm_servo_arm(false, channel_mask);
|
||||
}
|
||||
|
||||
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
|
||||
{
|
||||
if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* Allow a rate of 0 to enter oneshot mode */
|
||||
|
||||
if (rate != 0) {
|
||||
|
||||
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
|
||||
|
||||
if (rate < 1) {
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
if (rate > 10000) {
|
||||
return -ERANGE;
|
||||
}
|
||||
}
|
||||
|
||||
return io_timer_set_rate(group, rate);
|
||||
}
|
||||
|
||||
void up_pwm_update(unsigned channels_mask)
|
||||
{
|
||||
io_timer_trigger();
|
||||
}
|
||||
|
||||
int up_pwm_servo_set_rate(unsigned rate)
|
||||
{
|
||||
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
|
||||
up_pwm_servo_set_rate_group_update(i, rate);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t up_pwm_servo_get_rate_group(unsigned group)
|
||||
{
|
||||
return io_timer_get_group(group);
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_arm(bool armed, uint32_t channel_mask)
|
||||
{
|
||||
io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
|
||||
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
|
||||
}
|
|
@ -0,0 +1,47 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <px4_arch/micro_hal.h>
|
||||
#include <errno.h>
|
||||
|
||||
int rp2040_gpioconfig(uint32_t pinset)
|
||||
{
|
||||
if ((pinset & GPIO_NUM_MASK) > RP2040_GPIO_NUM) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
rp2040_gpio_set_pulls(pinset & GPIO_NUM_MASK, pinset & GPIO_PU_MASK, pinset & GPIO_PD_MASK);
|
||||
|
||||
if ((pinset & GPIO_FUN_MASK) >> 9 == RP2040_GPIO_FUNC_SIO) {
|
||||
rp2040_gpio_setdir(pinset & GPIO_NUM_MASK, pinset & GPIO_OUT_MASK);
|
||||
rp2040_gpio_put(pinset & GPIO_NUM_MASK, pinset & GPIO_SET_MASK);
|
||||
}
|
||||
|
||||
rp2040_gpio_set_function(pinset & GPIO_NUM_MASK, (pinset & GPIO_FUN_MASK) >> 9);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
// Be careful when using this function. Current nuttx implementation allows for only one type of interrupt
|
||||
// (out of four types rising, falling, level high, level low) to be active at a time.
|
||||
int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg)
|
||||
{
|
||||
int ret = -ENOSYS;
|
||||
|
||||
if (fallingedge & event & (func != NULL)) {
|
||||
ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, func, arg);
|
||||
rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK);
|
||||
|
||||
} else if (risingedge & event & (func != NULL)) {
|
||||
ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_HIGH, func, arg);
|
||||
rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK);
|
||||
|
||||
} else {
|
||||
rp2040_gpio_disable_irq(pinset & GPIO_NUM_MASK);
|
||||
ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, NULL, NULL);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
|
@ -0,0 +1,37 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_spi
|
||||
spi.cpp
|
||||
)
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
|
@ -0,0 +1,329 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <board_config.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <px4_platform_common/spi.h>
|
||||
#include <px4_arch/micro_hal.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <arm_arch.h>
|
||||
#include <chip.h>
|
||||
|
||||
static const px4_spi_bus_t *_spi_bus0;
|
||||
static const px4_spi_bus_t *_spi_bus1;
|
||||
|
||||
static void spi_bus_configgpio_cs(const px4_spi_bus_t *bus)
|
||||
{
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (bus->devices[i].cs_gpio != 0) {
|
||||
px4_arch_configgpio(bus->devices[i].cs_gpio | GPIO_FUN(RP2040_GPIO_FUNC_SIO));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void rp2040_spiinitialize()
|
||||
{
|
||||
px4_set_spi_buses_from_hw_version();
|
||||
board_control_spi_sensors_power_configgpio();
|
||||
board_control_spi_sensors_power(true, 0xffff);
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
|
||||
switch (px4_spi_buses[i].bus) {
|
||||
case PX4_BUS_NUMBER_TO_PX4(0): _spi_bus0 = &px4_spi_buses[i]; break;
|
||||
|
||||
case PX4_BUS_NUMBER_TO_PX4(1): _spi_bus1 = &px4_spi_buses[i]; break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Set default SPI pin */
|
||||
#if defined(CONFIG_RP2040_SPI0) && defined(GPIO_SPI0_SCLK) && defined(GPIO_SPI0_MISO) && defined(GPIO_SPI0_MOSI)
|
||||
px4_arch_configgpio(GPIO_SPI0_SCLK);
|
||||
px4_arch_configgpio(GPIO_SPI0_MISO);
|
||||
px4_arch_configgpio(GPIO_SPI0_MOSI);
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_RP2040_SPI1) && defined(GPIO_SPI1_SCLK) && defined(GPIO_SPI1_MISO) && defined(GPIO_SPI1_MOSI)
|
||||
px4_arch_configgpio(GPIO_SPI1_SCLK);
|
||||
px4_arch_configgpio(GPIO_SPI1_MISO);
|
||||
px4_arch_configgpio(GPIO_SPI1_MOSI);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_RP2040_SPI0
|
||||
ASSERT(_spi_bus0);
|
||||
|
||||
if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(0))) {
|
||||
spi_bus_configgpio_cs(_spi_bus0);
|
||||
}
|
||||
|
||||
#endif // CONFIG_RP2040_SPI0
|
||||
|
||||
#ifdef CONFIG_RP2040_SPI1
|
||||
ASSERT(_spi_bus1);
|
||||
|
||||
if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(1))) {
|
||||
spi_bus_configgpio_cs(_spi_bus1);
|
||||
}
|
||||
|
||||
#endif // CONFIG_RP2040_SPI1
|
||||
}
|
||||
|
||||
static inline void rp2040_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (bus->devices[i].cs_gpio == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (devid == bus->devices[i].devid) {
|
||||
// SPI select is active low, so write !selected to select the device
|
||||
px4_arch_gpiowrite(bus->devices[i].cs_gpio, !selected);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Name: rp2040_spi0/1select and rp2040_spi0/1status
|
||||
*
|
||||
* Description:
|
||||
* The external functions, rp2040_spi0/1select and rp2040_spi0/1status
|
||||
* must be provided by board-specific logic.
|
||||
* They are implementations of the select and status methods of the SPI
|
||||
* interface defined by struct spi_ops_s (see include/nuttx/spi/spi.h).
|
||||
* All other methods (including rp2040_spibus_initialize()) are provided by
|
||||
* common RP2040 logic. To use this common SPI logic on your board:
|
||||
*
|
||||
* 1. Provide logic in rp2040_boardinitialize() to configure SPI chip
|
||||
* select pins.
|
||||
* 2. Provide rp2040_spi0/1select() and rp2040_spi0/1status()
|
||||
* functions in your board-specific logic.
|
||||
* These functions will perform chip selection and status operations
|
||||
* using GPIOs in the way your board is configured.
|
||||
* 3. Add a calls to rp2040_spibus_initialize() in your low level
|
||||
* application initialization logic
|
||||
* 4. The handle returned by rp2040_spibus_initialize() may then be used to
|
||||
* bind the SPI driver to higher level logic (e.g., calling
|
||||
* mmcsd_spislotinitialize(), for example, will bind the SPI driver to
|
||||
* the SPI MMC/SD driver).
|
||||
*
|
||||
****************************************************************************/
|
||||
#ifdef CONFIG_RP2040_SPI0
|
||||
void rp2040_spi0select(FAR struct spi_dev_s *dev, uint32_t devid,
|
||||
bool selected)
|
||||
{
|
||||
spiinfo("devid: %d CS: %s\n", (int)devid,
|
||||
selected ? "assert" : "de-assert");
|
||||
rp2040_spixselect(_spi_bus0, dev, devid, selected);
|
||||
}
|
||||
|
||||
uint8_t rp2040_spi0status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_RP2040_SPI1
|
||||
void rp2040_spi1select(FAR struct spi_dev_s *dev, uint32_t devid,
|
||||
bool selected)
|
||||
{
|
||||
spiinfo("devid: %d CS: %s\n", (int)devid,
|
||||
selected ? "assert" : "de-assert");
|
||||
rp2040_spixselect(_spi_bus1, dev, devid, selected);
|
||||
}
|
||||
|
||||
uint8_t rp2040_spi1status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
void board_control_spi_sensors_power(bool enable_power, int bus_mask)
|
||||
{
|
||||
const px4_spi_bus_t *buses = px4_spi_buses;
|
||||
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
if (buses[bus].bus == -1) {
|
||||
break;
|
||||
}
|
||||
|
||||
const bool bus_matches = bus_mask & (1 << (buses[bus].bus - 1));
|
||||
|
||||
if (buses[bus].power_enable_gpio == 0 ||
|
||||
!board_has_bus(BOARD_SPI_BUS, buses[bus].bus) ||
|
||||
!bus_matches) {
|
||||
continue;
|
||||
}
|
||||
|
||||
px4_arch_gpiowrite(buses[bus].power_enable_gpio, enable_power ? 1 : 0);
|
||||
}
|
||||
}
|
||||
|
||||
void board_control_spi_sensors_power_configgpio()
|
||||
{
|
||||
const px4_spi_bus_t *buses = px4_spi_buses;
|
||||
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
if (buses[bus].bus == -1) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (buses[bus].power_enable_gpio == 0 ||
|
||||
!board_has_bus(BOARD_SPI_BUS, buses[bus].bus)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
px4_arch_configgpio(buses[bus].power_enable_gpio);
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void board_spi_reset(int ms, int bus_mask)
|
||||
{
|
||||
bool has_power_enable = false;
|
||||
|
||||
// disable SPI bus
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
if (px4_spi_buses[bus].bus == -1) {
|
||||
break;
|
||||
}
|
||||
|
||||
const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1));
|
||||
|
||||
if (px4_spi_buses[bus].power_enable_gpio == 0 ||
|
||||
!board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) ||
|
||||
!bus_requested) {
|
||||
continue;
|
||||
}
|
||||
|
||||
has_power_enable = true;
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio));
|
||||
}
|
||||
|
||||
if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio));
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_RP2040_SPI0)
|
||||
|
||||
if (px4_spi_buses[bus].bus == 1) {
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_SCLK));
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MISO));
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MOSI));
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(CONFIG_RP2040_SPI1)
|
||||
|
||||
if (px4_spi_buses[bus].bus == 2) {
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_SCLK));
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MISO));
|
||||
px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MOSI));
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
if (!has_power_enable) {
|
||||
// board does not have power control over any of the sensor buses
|
||||
return;
|
||||
}
|
||||
|
||||
// set the sensor rail(s) off
|
||||
board_control_spi_sensors_power(false, bus_mask);
|
||||
|
||||
// wait for the sensor rail to reach GND
|
||||
usleep(ms * 1000);
|
||||
syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
|
||||
|
||||
/* re-enable power */
|
||||
|
||||
// switch the sensor rail back on
|
||||
board_control_spi_sensors_power(true, bus_mask);
|
||||
|
||||
/* wait a bit before starting SPI, different times didn't influence results */
|
||||
usleep(100);
|
||||
|
||||
/* reconfigure the SPI pins */
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
if (px4_spi_buses[bus].bus == -1) {
|
||||
break;
|
||||
}
|
||||
|
||||
const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1));
|
||||
|
||||
if (px4_spi_buses[bus].power_enable_gpio == 0 ||
|
||||
!board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) ||
|
||||
!bus_requested) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
|
||||
px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio);
|
||||
}
|
||||
|
||||
if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
|
||||
px4_arch_configgpio(px4_spi_buses[bus].devices[i].drdy_gpio);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_RP2040_SPI0)
|
||||
|
||||
if (px4_spi_buses[bus].bus == 1) {
|
||||
px4_arch_configgpio(GPIO_SPI0_SCLK);
|
||||
px4_arch_configgpio(GPIO_SPI0_MISO);
|
||||
px4_arch_configgpio(GPIO_SPI0_MOSI);
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(CONFIG_RP2040_SPI1)
|
||||
|
||||
if (px4_spi_buses[bus].bus == 2) {
|
||||
px4_arch_configgpio(GPIO_SPI1_SCLK);
|
||||
px4_arch_configgpio(GPIO_SPI1_MISO);
|
||||
px4_arch_configgpio(GPIO_SPI1_MOSI);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
|
@ -0,0 +1,37 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_version
|
||||
board_identity.c
|
||||
board_mcu_version.c
|
||||
)
|
|
@ -0,0 +1,146 @@
|
|||
/**
|
||||
* @file board_identity.c
|
||||
* Implementation of RP2040 based Board identity API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
// RP2040 doesn't really have a cpu register with unique id.
|
||||
// However, there is a function in pico-sdk which can provide
|
||||
// a device unique id from its flash which is 64 bits in length.
|
||||
// For now, a fixed value of 12 bytes "PIPICORP2040" is used.
|
||||
uint32_t myUUID[3] = {'P' << 0 | 'I' << 8 | 'P' << 16 | 'I' << 24,
|
||||
'C' << 0 | 'O' << 8 | 'R' << 16 | 'P' << 24,
|
||||
'2' << 0 | '0' << 8 | '4' << 16 | '0' << 24
|
||||
};
|
||||
#define RP2040_SYSTEM_UID ((uint32_t)myUUID)
|
||||
|
||||
#define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8}
|
||||
#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 chips non-write memory */
|
||||
|
||||
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]];
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words)
|
||||
{
|
||||
uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
|
||||
|
||||
for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
uuid_words[i] = chip_uuid[i];
|
||||
}
|
||||
}
|
||||
|
||||
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 i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
offset += snprintf(&format_buffer[offset], size - offset, 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)
|
||||
{
|
||||
uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
|
||||
uint8_t *rv = &mfgid[0];
|
||||
|
||||
for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]);
|
||||
memcpy(rv, &uuid_bytes, sizeof(uint32_t));
|
||||
rv += sizeof(uint32_t);
|
||||
}
|
||||
|
||||
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 i = 0; offset < size && 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;
|
||||
}
|
||||
|
||||
uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
|
||||
|
||||
for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]);
|
||||
memcpy(pb, &uuid_bytes, sizeof(uint32_t));
|
||||
pb += sizeof(uint32_t);
|
||||
}
|
||||
|
||||
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,101 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file board_mcu_version.c
|
||||
* Implementation of RP2040 based SoC version API
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#define RP2040_CPUID_BASE (RP2040_PPB_BASE + 0xed00)
|
||||
|
||||
/* magic numbers from reference manual */
|
||||
|
||||
enum MCU_REV {
|
||||
MCU_REV_RP2040_REV_1 = 0x1
|
||||
};
|
||||
|
||||
/* Define any issues with the Silicon as lines separated by \n
|
||||
* omitting the last \n
|
||||
*/
|
||||
#define RP2040_ERRATA "This device does not have a unique id!"
|
||||
|
||||
|
||||
// RP2040 datasheet CPUID register
|
||||
# define REVID_MASK 0xF
|
||||
# define DEVID_MASK 0xFFFFFFF0
|
||||
|
||||
# define RP2040_DEVICE_ID 0x410CC60
|
||||
|
||||
|
||||
int board_mcu_version(char *rev, const char **revstr, const char **errata)
|
||||
{
|
||||
uint32_t abc = getreg32(RP2040_CPUID_BASE);
|
||||
|
||||
int32_t chip_version = (abc & DEVID_MASK) > 4;
|
||||
enum MCU_REV revid = abc & REVID_MASK;
|
||||
const char *chip_errata = NULL;
|
||||
|
||||
switch (chip_version) {
|
||||
|
||||
|
||||
case RP2040_DEVICE_ID:
|
||||
*revstr = "RP2040";
|
||||
chip_errata = RP2040_ERRATA;
|
||||
break;
|
||||
|
||||
default:
|
||||
*revstr = "RPI???";
|
||||
break;
|
||||
}
|
||||
|
||||
switch (revid) {
|
||||
|
||||
case MCU_REV_RP2040_REV_1:
|
||||
*rev = '1';
|
||||
break;
|
||||
|
||||
default:
|
||||
*rev = '?';
|
||||
revid = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
if (errata) {
|
||||
*errata = chip_errata;
|
||||
}
|
||||
|
||||
return revid;
|
||||
}
|
|
@ -225,7 +225,9 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
|
|||
|
||||
/* if we have already retried once, or we are going to give up, then reset the bus */
|
||||
if ((retry_count >= 1) || (retry_count >= _retries)) {
|
||||
#if defined(CONFIG_I2C_RESET)
|
||||
I2C_RESET(_dev);
|
||||
#endif // CONFIG_I2C_RESET
|
||||
}
|
||||
|
||||
} while (retry_count++ < _retries);
|
||||
|
|
|
@ -105,7 +105,9 @@ int detect(int bus)
|
|||
|
||||
// if we have already retried once, or we are going to give up, then reset the bus
|
||||
if ((retry_count >= 1) || (retry_count >= retries)) {
|
||||
#if defined(CONFIG_I2C_RESET)
|
||||
I2C_RESET(i2c_dev);
|
||||
#endif // CONFIG_I2C_RESET
|
||||
}
|
||||
|
||||
} while (retry_count++ < retries);
|
||||
|
|
Loading…
Reference in New Issue