From ea1ae73526c11a91ffef0329035ea8fbca2fdb9c Mon Sep 17 00:00:00 2001 From: Vatsal Asitkumar Joshi <45529441+vxj9800@users.noreply.github.com> Date: Wed, 3 Nov 2021 11:14:30 -0500 Subject: [PATCH] Support for Raspberry PI RP2040 MCU (#18083) --- .ci/Jenkinsfile-compile | 1 + .github/workflows/compile_nuttx.yml | 1 + .vscode/cmake-variants.yaml | 5 + .vscode/settings.json | 3 +- boards/raspberrypi/pico/default.px4board | 52 ++ boards/raspberrypi/pico/firmware.prototype | 13 + .../raspberrypi/pico/init/rc.board_defaults | 18 + boards/raspberrypi/pico/init/rc.board_mavlink | 7 + boards/raspberrypi/pico/init/rc.board_sensors | 12 + boards/raspberrypi/pico/nuttx-config/Kconfig | 25 + .../pico/nuttx-config/include/board.h | 129 +++ .../pico/nuttx-config/nsh/defconfig | 118 +++ .../pico/nuttx-config/scripts/flash.ld | 113 +++ .../pico/nuttx-config/scripts/script.ld | 113 +++ boards/raspberrypi/pico/src/CMakeLists.txt | 52 ++ boards/raspberrypi/pico/src/board_config.h | 140 +++ boards/raspberrypi/pico/src/boot_string.c | 20 + boards/raspberrypi/pico/src/i2c.cpp | 38 + boards/raspberrypi/pico/src/init.c | 399 +++++++++ boards/raspberrypi/pico/src/led.c | 119 +++ boards/raspberrypi/pico/src/spi.cpp | 48 ++ boards/raspberrypi/pico/src/timer_config.cpp | 49 ++ boards/raspberrypi/pico/src/usb.c | 85 ++ ...eric-arm-none-eabi-gcc-cortex-m0plus.cmake | 5 + platforms/nuttx/cmake/px4_impl_os.cmake | 3 + platforms/nuttx/src/px4/rpi/CMakeLists.txt | 1 + .../nuttx/src/px4/rpi/rp2040/CMakeLists.txt | 7 + .../src/px4/rpi/rp2040/include/px4_arch/adc.h | 36 + .../rp2040/include/px4_arch/hw_description.h | 36 + .../include/px4_arch/i2c_hw_description.h | 36 + .../rpi/rp2040/include/px4_arch/io_timer.h | 36 + .../px4_arch/io_timer_hw_description.h | 36 + .../rpi/rp2040/include/px4_arch/micro_hal.h | 12 + .../include/px4_arch/spi_hw_description.h | 66 ++ .../src/px4/rpi/rpi_common/adc/CMakeLists.txt | 36 + .../nuttx/src/px4/rpi/rpi_common/adc/adc.cpp | 176 ++++ .../rpi_common/board_critmon/CMakeLists.txt | 36 + .../rpi_common/board_critmon/board_critmon.c | 66 ++ .../rpi/rpi_common/board_reset/CMakeLists.txt | 36 + .../rpi_common/board_reset/board_reset.cpp | 158 ++++ .../src/px4/rpi/rpi_common/hrt/CMakeLists.txt | 41 + .../nuttx/src/px4/rpi/rpi_common/hrt/hrt.c | 809 ++++++++++++++++++ .../px4/rpi/rpi_common/include/px4_arch/adc.h | 40 + .../include/px4_arch/hw_description.h | 222 +++++ .../include/px4_arch/i2c_hw_description.h | 54 ++ .../rpi_common/include/px4_arch/io_timer.h | 163 ++++ .../px4_arch/io_timer_hw_description.h | 157 ++++ .../rpi_common/include/px4_arch/micro_hal.h | 107 +++ .../include/px4_arch/spi_hw_description.h | 122 +++ .../px4/rpi/rpi_common/io_pins/CMakeLists.txt | 42 + .../src/px4/rpi/rpi_common/io_pins/io_timer.c | 768 +++++++++++++++++ .../px4/rpi/rpi_common/io_pins/pwm_servo.c | 158 ++++ .../rpi/rpi_common/io_pins/rp2040_pinset.c | 47 + .../src/px4/rpi/rpi_common/spi/CMakeLists.txt | 37 + .../nuttx/src/px4/rpi/rpi_common/spi/spi.cpp | 329 +++++++ .../px4/rpi/rpi_common/version/CMakeLists.txt | 37 + .../rpi/rpi_common/version/board_identity.c | 146 ++++ .../rpi_common/version/board_mcu_version.c | 101 +++ src/lib/drivers/device/nuttx/I2C.cpp | 2 + src/systemcmds/i2cdetect/i2cdetect.cpp | 2 + 60 files changed, 5725 insertions(+), 1 deletion(-) create mode 100644 boards/raspberrypi/pico/default.px4board create mode 100644 boards/raspberrypi/pico/firmware.prototype create mode 100644 boards/raspberrypi/pico/init/rc.board_defaults create mode 100644 boards/raspberrypi/pico/init/rc.board_mavlink create mode 100644 boards/raspberrypi/pico/init/rc.board_sensors create mode 100644 boards/raspberrypi/pico/nuttx-config/Kconfig create mode 100644 boards/raspberrypi/pico/nuttx-config/include/board.h create mode 100644 boards/raspberrypi/pico/nuttx-config/nsh/defconfig create mode 100644 boards/raspberrypi/pico/nuttx-config/scripts/flash.ld create mode 100644 boards/raspberrypi/pico/nuttx-config/scripts/script.ld create mode 100644 boards/raspberrypi/pico/src/CMakeLists.txt create mode 100644 boards/raspberrypi/pico/src/board_config.h create mode 100644 boards/raspberrypi/pico/src/boot_string.c create mode 100644 boards/raspberrypi/pico/src/i2c.cpp create mode 100644 boards/raspberrypi/pico/src/init.c create mode 100644 boards/raspberrypi/pico/src/led.c create mode 100644 boards/raspberrypi/pico/src/spi.cpp create mode 100644 boards/raspberrypi/pico/src/timer_config.cpp create mode 100644 boards/raspberrypi/pico/src/usb.c create mode 100644 platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake create mode 100644 platforms/nuttx/src/px4/rpi/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h create mode 100644 platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c create mode 100644 platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index e0a7763779..d7554180fb 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -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", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 8b4426910e..320f2f0b89 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -48,6 +48,7 @@ jobs: nxp_fmurt1062-v1, nxp_ucans32k146, omnibus_f4sd, + raspberrypi_pico, px4_fmu-v2, px4_fmu-v3, px4_fmu-v4, diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 4f1544ac6b..0791d72782 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -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 diff --git a/.vscode/settings.json b/.vscode/settings.json index 75e60d908e..1d35607955 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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 } diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board new file mode 100644 index 0000000000..75c0ae9d8b --- /dev/null +++ b/boards/raspberrypi/pico/default.px4board @@ -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 diff --git a/boards/raspberrypi/pico/firmware.prototype b/boards/raspberrypi/pico/firmware.prototype new file mode 100644 index 0000000000..1fcf0a363b --- /dev/null +++ b/boards/raspberrypi/pico/firmware.prototype @@ -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 +} diff --git a/boards/raspberrypi/pico/init/rc.board_defaults b/boards/raspberrypi/pico/init/rc.board_defaults new file mode 100644 index 0000000000..641d73110c --- /dev/null +++ b/boards/raspberrypi/pico/init/rc.board_defaults @@ -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 diff --git a/boards/raspberrypi/pico/init/rc.board_mavlink b/boards/raspberrypi/pico/init/rc.board_mavlink new file mode 100644 index 0000000000..75c5256651 --- /dev/null +++ b/boards/raspberrypi/pico/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the USB port +mavlink start -d /dev/ttyACM0 diff --git a/boards/raspberrypi/pico/init/rc.board_sensors b/boards/raspberrypi/pico/init/rc.board_sensors new file mode 100644 index 0000000000..2246747096 --- /dev/null +++ b/boards/raspberrypi/pico/init/rc.board_sensors @@ -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 diff --git a/boards/raspberrypi/pico/nuttx-config/Kconfig b/boards/raspberrypi/pico/nuttx-config/Kconfig new file mode 100644 index 0000000000..f70b26deec --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/Kconfig @@ -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. diff --git a/boards/raspberrypi/pico/nuttx-config/include/board.h b/boards/raspberrypi/pico/nuttx-config/include/board.h new file mode 100644 index 0000000000..fe16b07ec8 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/include/board.h @@ -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 +#ifndef __ASSEMBLY__ +# include +#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 */ diff --git a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..8aae9f5c7a --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig @@ -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 diff --git a/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld b/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld new file mode 100644 index 0000000000..1193a1f5d7 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld @@ -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) } +} diff --git a/boards/raspberrypi/pico/nuttx-config/scripts/script.ld b/boards/raspberrypi/pico/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..1193a1f5d7 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/scripts/script.ld @@ -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) } +} diff --git a/boards/raspberrypi/pico/src/CMakeLists.txt b/boards/raspberrypi/pico/src/CMakeLists.txt new file mode 100644 index 0000000000..9fc76faced --- /dev/null +++ b/boards/raspberrypi/pico/src/CMakeLists.txt @@ -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 +) diff --git a/boards/raspberrypi/pico/src/board_config.h b/boards/raspberrypi/pico/src/board_config.h new file mode 100644 index 0000000000..9ecc797043 --- /dev/null +++ b/boards/raspberrypi/pico/src/board_config.h @@ -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 +#include +#include + +/* 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 + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/raspberrypi/pico/src/boot_string.c b/boards/raspberrypi/pico/src/boot_string.c new file mode 100644 index 0000000000..3b3b2ec4b2 --- /dev/null +++ b/boards/raspberrypi/pico/src/boot_string.c @@ -0,0 +1,20 @@ +#include + +__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 + }; diff --git a/boards/raspberrypi/pico/src/i2c.cpp b/boards/raspberrypi/pico/src/i2c.cpp new file mode 100644 index 0000000000..4ab0a07f67 --- /dev/null +++ b/boards/raspberrypi/pico/src/i2c.cpp @@ -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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), +}; diff --git a/boards/raspberrypi/pico/src/init.c b/boards/raspberrypi/pico/src/init.c new file mode 100644 index 0000000000..fe8c188691 --- /dev/null +++ b/boards/raspberrypi/pico/src/init.c @@ -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 +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "board_config.h" +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#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; +} diff --git a/boards/raspberrypi/pico/src/led.c b/boards/raspberrypi/pico/src/led.c new file mode 100644 index 0000000000..2e92025b4b --- /dev/null +++ b/boards/raspberrypi/pico/src/led.c @@ -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 + +#include + +#include "board_config.h" + +#include + +/* + * 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); +// } +// } diff --git a/boards/raspberrypi/pico/src/spi.cpp b/boards/raspberrypi/pico/src/spi.cpp new file mode 100644 index 0000000000..a258dec42d --- /dev/null +++ b/boards/raspberrypi/pico/src/spi.cpp @@ -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 +#include +#include + +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); diff --git a/boards/raspberrypi/pico/src/timer_config.cpp b/boards/raspberrypi/pico/src/timer_config.cpp new file mode 100644 index 0000000000..4cf036efd4 --- /dev/null +++ b/boards/raspberrypi/pico/src/timer_config.cpp @@ -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 + +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); diff --git a/boards/raspberrypi/pico/src/usb.c b/boards/raspberrypi/pico/src/usb.c new file mode 100644 index 0000000000..393f80960e --- /dev/null +++ b/boards/raspberrypi/pico/src/usb.c @@ -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 + +#include +#include +#include +#include + +#include +#include + +#include +#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); +} + diff --git a/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake b/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake new file mode 100644 index 0000000000..9f36102bb7 --- /dev/null +++ b/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake @@ -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) diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 67ce2e7700..e5a04a3cfc 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -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() diff --git a/platforms/nuttx/src/px4/rpi/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/CMakeLists.txt new file mode 100644 index 0000000000..9d3491e36e --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt new file mode 100644 index 0000000000..a6f9e1497e --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt @@ -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) diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h new file mode 100644 index 0000000000..234971efbd --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/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/adc.h" + diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.h new file mode 100644 index 0000000000..1262b2407c --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/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/hw_description.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h new file mode 100644 index 0000000000..9655c49558 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/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/i2c_hw_description.h" + diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.h new file mode 100644 index 0000000000..e08db25452 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/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.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 0000000000..8065d43bfa --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_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_hw_description.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h new file mode 100644 index 0000000000..bb38993703 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.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 diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h new file mode 100644 index 0000000000..4c62007715 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h @@ -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; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt new file mode 100644 index 0000000000..8f0546f5ee --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt @@ -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 +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp new file mode 100644 index 0000000000..d068fea8eb --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/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 +#include +#include +#include +#include + +// #include Nuttx doesn't have this file in arch yet. +#include + +/* + * 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 +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt new file mode 100644 index 0000000000..3edcbee062 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt @@ -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 +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c new file mode 100644 index 0000000000..14e7a3bb3b --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c @@ -0,0 +1,66 @@ +/************************************************************************************ + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "missing implementation for up_critmon_gettime() and up_critmon_convert()" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +// uint32_t up_critmon_gettime(void) +// { +// } + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +// void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +// { +// } + +#endif /* CONFIG_SCHED_CRITMONITOR */ diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt new file mode 100644 index 0000000000..c4163fb3fa --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt @@ -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 +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp new file mode 100644 index 0000000000..058f07aea1 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_reset.cpp + * Implementation of RP2040 based Board RESET API + */ + +#include +#include +#include + +#include + +// 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 diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt new file mode 100644 index 0000000000..9ce26ff632 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt @@ -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 +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c new file mode 100644 index 0000000000..dcbf75645d --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c @@ -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 +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + + +// #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 */ diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h new file mode 100644 index 0000000000..96aa40bc33 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h @@ -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 + +#define SYSTEM_ADC_BASE RP2040_ADC_BASE + +#include + diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h new file mode 100644 index 0000000000..5588a45c70 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.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 + +#include + +#include + + +/* + * 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 diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h new file mode 100644 index 0000000000..5254f9effa --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h @@ -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 +#include + + +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; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h new file mode 100644 index 0000000000..e5901703a2 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -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 +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#ifdef BOARD_NUM_IO_TIMERS +#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS +#else +#define MAX_IO_TIMERS 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 diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 0000000000..e76cddff4e --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h @@ -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 +#include +#include +#include +#include + +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; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h new file mode 100644 index 0000000000..1188b0704e --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h @@ -0,0 +1,107 @@ +#pragma once + +#include + +__BEGIN_DECLS + +#include +#include +#include + +// 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 diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h new file mode 100644 index 0000000000..4279b955b7 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h @@ -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 +#include +#include + +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]); diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt new file mode 100644 index 0000000000..a5bd3341b0 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt @@ -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}) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c new file mode 100644 index 0000000000..b519a8da24 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c @@ -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 +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +// 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); + +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c new file mode 100644 index 0000000000..b8ab8db721 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c @@ -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 +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_free_channel(channel); + current &= ~(1 << channel); + } + } + + // Now allocate the new set + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not PWM mode before + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + return OK; +} + +void up_pwm_servo_deinit(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); +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c new file mode 100644 index 0000000000..04865ce179 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c @@ -0,0 +1,47 @@ +#include +#include + +#include + +#include +#include + +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; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt new file mode 100644 index 0000000000..dda6d7c98e --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt @@ -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}) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp new file mode 100644 index 0000000000..350b914342 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp @@ -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 +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +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 + } +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt new file mode 100644 index 0000000000..b102ab612f --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt @@ -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 +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c new file mode 100644 index 0000000000..677c3eb8a1 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c @@ -0,0 +1,146 @@ +/** + * @file board_identity.c + * Implementation of RP2040 based Board identity API + */ + +#include +#include +#include + +// 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; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c new file mode 100644 index 0000000000..646eab9185 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c @@ -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 +#include + +#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; +} diff --git a/src/lib/drivers/device/nuttx/I2C.cpp b/src/lib/drivers/device/nuttx/I2C.cpp index c2ffb06c04..70de19dac8 100644 --- a/src/lib/drivers/device/nuttx/I2C.cpp +++ b/src/lib/drivers/device/nuttx/I2C.cpp @@ -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); diff --git a/src/systemcmds/i2cdetect/i2cdetect.cpp b/src/systemcmds/i2cdetect/i2cdetect.cpp index 74323e6305..f02f3570e3 100644 --- a/src/systemcmds/i2cdetect/i2cdetect.cpp +++ b/src/systemcmds/i2cdetect/i2cdetect.cpp @@ -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);