From ecf2cd3afbf4b8633a44b4f580dfd2b4c830b934 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 16 May 2021 13:10:02 -0400 Subject: [PATCH] CAN node STM32F7 support and Freefly RTK GPS CAN node --- .ci/Jenkinsfile-compile | 2 + .github/workflows/compile_nuttx.yml | 2 + .github/workflows/compile_nuttx_cannode.yml | 1 + .vscode/cmake-variants.yaml | 10 + .../freefly/can-rtk-gps/canbootloader.cmake | 14 + boards/freefly/can-rtk-gps/default.cmake | 37 ++ boards/freefly/can-rtk-gps/firmware.prototype | 13 + .../freefly/can-rtk-gps/init/rc.board_sensors | 14 + .../nuttx-config/canbootloader/defconfig | 67 +++ .../can-rtk-gps/nuttx-config/include/board.h | 267 +++++++++ .../nuttx-config/include/board_dma_map.h | 102 ++++ .../can-rtk-gps/nuttx-config/nsh/defconfig | 175 ++++++ .../scripts/canbootloader_script.ld | 155 +++++ .../nuttx-config/scripts/script.ld | 169 ++++++ boards/freefly/can-rtk-gps/src/CMakeLists.txt | 67 +++ boards/freefly/can-rtk-gps/src/board_config.h | 72 +++ boards/freefly/can-rtk-gps/src/boot.c | 192 ++++++ boards/freefly/can-rtk-gps/src/boot_config.h | 134 +++++ boards/freefly/can-rtk-gps/src/can.c | 127 ++++ boards/freefly/can-rtk-gps/src/i2c.cpp | 39 ++ boards/freefly/can-rtk-gps/src/init.c | 148 +++++ boards/freefly/can-rtk-gps/src/led.cpp | 239 ++++++++ boards/freefly/can-rtk-gps/src/led.h | 37 ++ boards/freefly/can-rtk-gps/src/spi.cpp | 44 ++ boards/freefly/can-rtk-gps/src/usb.c | 93 +++ .../freefly/can-rtk-gps/uavcan_board_identity | 17 + platforms/nuttx/CMakeLists.txt | 4 + .../nuttx/src/canbootloader/CMakeLists.txt | 1 + .../arch/stm/stm32f7/CMakeLists.txt | 44 ++ .../arch/stm/stm32f7/board_identity.c | 56 ++ .../arch/stm/stm32f7/drivers/can/driver.c | 566 ++++++++++++++++++ .../src/canbootloader/common/nuttx_stubs.c | 23 +- .../src/canbootloader/include/cxx_init.h | 51 ++ .../nuttx/src/canbootloader/util/cxx_init.c | 126 ++++ .../nuttx/src/px4/stm/stm32f7/CMakeLists.txt | 2 +- .../stm/stm32f7/include/px4_arch/micro_hal.h | 10 +- .../px4/stm/stm32f7/watchdog/CMakeLists.txt | 36 ++ .../nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c | 104 ++++ .../src/px4/stm/stm32f7/watchdog/stm32_wdg.h | 162 +++++ 39 files changed, 3417 insertions(+), 5 deletions(-) create mode 100644 boards/freefly/can-rtk-gps/canbootloader.cmake create mode 100644 boards/freefly/can-rtk-gps/default.cmake create mode 100644 boards/freefly/can-rtk-gps/firmware.prototype create mode 100644 boards/freefly/can-rtk-gps/init/rc.board_sensors create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/include/board.h create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld create mode 100644 boards/freefly/can-rtk-gps/nuttx-config/scripts/script.ld create mode 100644 boards/freefly/can-rtk-gps/src/CMakeLists.txt create mode 100644 boards/freefly/can-rtk-gps/src/board_config.h create mode 100644 boards/freefly/can-rtk-gps/src/boot.c create mode 100644 boards/freefly/can-rtk-gps/src/boot_config.h create mode 100644 boards/freefly/can-rtk-gps/src/can.c create mode 100644 boards/freefly/can-rtk-gps/src/i2c.cpp create mode 100644 boards/freefly/can-rtk-gps/src/init.c create mode 100644 boards/freefly/can-rtk-gps/src/led.cpp create mode 100644 boards/freefly/can-rtk-gps/src/led.h create mode 100644 boards/freefly/can-rtk-gps/src/spi.cpp create mode 100644 boards/freefly/can-rtk-gps/src/usb.c create mode 100644 boards/freefly/can-rtk-gps/uavcan_board_identity create mode 100644 platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt create mode 100644 platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c create mode 100644 platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c create mode 100644 platforms/nuttx/src/canbootloader/include/cxx_init.h create mode 100644 platforms/nuttx/src/canbootloader/util/cxx_init.c create mode 100644 platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c create mode 100644 platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 91c00385fb..6ea07cf989 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -48,6 +48,8 @@ pipeline { "cuav_x7pro_default", "cubepilot_cubeorange_default", "cubepilot_cubeyellow_default", + "freefly_can-rtk-gps_canbootloader", + "freefly_can-rtk-gps_default", "holybro_can-gps-v1_canbootloader", "holybro_can-gps-v1_default", "holybro_durandal-v1_default", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 17c4081011..e75aad45af 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -32,6 +32,8 @@ jobs: cubepilot_cubeyellow_default, cubepilot_cubeyellow_test, cubepilot_io-v2_default, + freefly_can-rtk-gps_canbootloader, + freefly_can-rtk-gps_default, holybro_can-gps-v1_canbootloader, holybro_can-gps-v1_debug, holybro_can-gps-v1_default, diff --git a/.github/workflows/compile_nuttx_cannode.yml b/.github/workflows/compile_nuttx_cannode.yml index beac02e61b..59163d33a8 100644 --- a/.github/workflows/compile_nuttx_cannode.yml +++ b/.github/workflows/compile_nuttx_cannode.yml @@ -17,6 +17,7 @@ jobs: config: [ ark_can-flow_default, cuav_can-gps-v1_default, + freefly_can-rtk-gps_default, holybro_can-gps-v1_default, #nxp_ucans32k146_default, px4_fmu-v4_cannode diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index e2e482f4ba..41f82c5b62 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -106,6 +106,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: emlid_navio2_default + freefly_can-rtk-gps_default: + short: freefly_can-rtk-gps_default + buildType: MinSizeRel + settings: + CONFIG: freefly_can-rtk-gps_default + freefly_can-rtk-gps_canbootloader: + short: freefly_can-rtk-gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: freefly_can-rtk-gps_canbootloader holybro_can-gps-v1_canbootloader: short: holybro_can-gps-v1_canbootloader buildType: MinSizeRel diff --git a/boards/freefly/can-rtk-gps/canbootloader.cmake b/boards/freefly/can-rtk-gps/canbootloader.cmake new file mode 100644 index 0000000000..00d56652b0 --- /dev/null +++ b/boards/freefly/can-rtk-gps/canbootloader.cmake @@ -0,0 +1,14 @@ +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) + +px4_add_board( + PLATFORM nuttx + VENDOR freefly + MODEL can-rtk-gps + LABEL canbootloader + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + CONSTRAINED_MEMORY + DRIVERS + bootloaders + lights/rgbled_ncp5623c +) diff --git a/boards/freefly/can-rtk-gps/default.cmake b/boards/freefly/can-rtk-gps/default.cmake new file mode 100644 index 0000000000..1e79514858 --- /dev/null +++ b/boards/freefly/can-rtk-gps/default.cmake @@ -0,0 +1,37 @@ +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) + +px4_add_board( + PLATFORM nuttx + VENDOR freefly + MODEL can-rtk-gps + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + #CONSTRAINED_FLASH + ROMFSROOT cannode + UAVCAN_INTERFACES 1 + DRIVERS + barometer/bmp388 + bootloaders + gps + lights/rgbled_ncp5623c + magnetometer/isentek/ist8310 + imu/st/lsm9ds1 + uavcannode + MODULES + #ekf2 + load_mon + sensors + SYSTEMCMDS + led_control + mft + mtd + param + perf + reboot + system_time + top +# topic_listener + ver + work_queue +) diff --git a/boards/freefly/can-rtk-gps/firmware.prototype b/boards/freefly/can-rtk-gps/firmware.prototype new file mode 100644 index 0000000000..3f8a8bf393 --- /dev/null +++ b/boards/freefly/can-rtk-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 85, + "magic": "PX4FWv1", + "description": "Firmware for the FreeFly RTK GPS", + "image": "", + "build_time": 0, + "summary": "FFRTK", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/freefly/can-rtk-gps/init/rc.board_sensors b/boards/freefly/can-rtk-gps/init/rc.board_sensors new file mode 100644 index 0000000000..e905822d32 --- /dev/null +++ b/boards/freefly/can-rtk-gps/init/rc.board_sensors @@ -0,0 +1,14 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +rgbled_ncp5623c -I -b 1 -a 0x39 start + +ist8310 start -I -b 1 -a 0x0E + +bmp388 start -I -b b -a 0x77 + +safety_button start + +gps start -d /dev/ttyS0 -g 115200 -p ubx diff --git a/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..c4f79b1a24 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,67 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="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="stm32f7" +CONFIG_ARCH_CHIP_STM32F722RE=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_POLLED=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_MAX_TASKS=0 +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32F7_CAN1=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=4096 diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h new file mode 100644 index 0000000000..0dbed5a979 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h @@ -0,0 +1,267 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 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. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 16 MHz Crystal + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 2 <= PLLM <= 63 + * 192 <= PLLN <= 432 + * 192 MHz <= PLL_VCO <= 432MHz + * + * SYSCLK = PLL_VCO / PLLP + * Subject to + * + * PLLP = {2, 4, 6, 8} + * SYSCLK <= 216 MHz + * + * USB OTG FS, SDMMC and RNG Clock = PLL_VCO / PLLQ + * Subject to + * The USB OTG FS requires a 48 MHz clock to work correctly. The SDMMC + * and the random number generator need a frequency lower than or equal + * to 48 MHz to work correctly. + * + * 2 <= PLLQ <= 15 + */ + +/* Highest SYSCLK with USB OTG FS clock = 48 MHz + * + * PLL_VCO = (16,000,000 / 8) * 216 = 432 MHz + * SYSCLK = 432 MHz / 2 = 216 MHz + * USB OTG FS, SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(216) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(9) + +#define STM32_VCO_FREQUENCY ((STM32_HSE_FREQUENCY / 8) * 216) +#define STM32_SYSCLK_FREQUENCY (STM32_VCO_FREQUENCY / 2) +#define STM32_OTGFS_FREQUENCY (STM32_VCO_FREQUENCY / 9) + +/* Configure factors for PLLSAI clock */ + +#define CONFIG_STM32F7_PLLSAI 1 +#define STM32_RCC_PLLSAICFGR_PLLSAIN RCC_PLLSAICFGR_PLLSAIN(192) +#define STM32_RCC_PLLSAICFGR_PLLSAIP RCC_PLLSAICFGR_PLLSAIP(8) +#define STM32_RCC_PLLSAICFGR_PLLSAIQ RCC_PLLSAICFGR_PLLSAIQ(4) +#define STM32_RCC_PLLSAICFGR_PLLSAIR RCC_PLLSAICFGR_PLLSAIR(2) + +/* Configure Dedicated Clock Configuration Register */ + +#define STM32_RCC_DCKCFGR1_PLLI2SDIVQ RCC_DCKCFGR1_PLLI2SDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVQ RCC_DCKCFGR1_PLLSAIDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVR RCC_DCKCFGR1_PLLSAIDIVR(0) +#define STM32_RCC_DCKCFGR1_SAI1SRC RCC_DCKCFGR1_SAI1SEL(0) +#define STM32_RCC_DCKCFGR1_SAI2SRC RCC_DCKCFGR1_SAI2SEL(0) +#define STM32_RCC_DCKCFGR1_TIMPRESRC 0 +#define STM32_RCC_DCKCFGR1_DFSDM1SRC 0 +#define STM32_RCC_DCKCFGR1_ADFSDM1SRC 0 + + + +/* Configure factors for PLLI2S clock */ + +#define CONFIG_STM32F7_PLLI2S 1 +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SP RCC_PLLI2SCFGR_PLLI2SP(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) + +/* Configure Dedicated Clock Configuration Register 2 */ + +#define STM32_RCC_DCKCFGR2_USART1SRC RCC_DCKCFGR2_USART1SEL_APB +#define STM32_RCC_DCKCFGR2_USART2SRC RCC_DCKCFGR2_USART2SEL_APB +#define STM32_RCC_DCKCFGR2_UART4SRC RCC_DCKCFGR2_UART4SEL_APB +#define STM32_RCC_DCKCFGR2_UART5SRC RCC_DCKCFGR2_UART5SEL_APB +#define STM32_RCC_DCKCFGR2_USART6SRC RCC_DCKCFGR2_USART6SEL_APB +#define STM32_RCC_DCKCFGR2_UART7SRC RCC_DCKCFGR2_UART7SEL_APB +#define STM32_RCC_DCKCFGR2_UART8SRC RCC_DCKCFGR2_UART8SEL_APB +#define STM32_RCC_DCKCFGR2_I2C1SRC RCC_DCKCFGR2_I2C1SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C2SRC RCC_DCKCFGR2_I2C2SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C3SRC RCC_DCKCFGR2_I2C3SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C4SRC RCC_DCKCFGR2_I2C4SEL_HSI +#define STM32_RCC_DCKCFGR2_LPTIM1SRC RCC_DCKCFGR2_LPTIM1SEL_APB +#define STM32_RCC_DCKCFGR2_CECSRC RCC_DCKCFGR2_CECSEL_HSI +#define STM32_RCC_DCKCFGR2_CK48MSRC RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_SDMMCSRC RCC_DCKCFGR2_SDMMCSEL_48MHZ +#define STM32_RCC_DCKCFGR2_SDMMC2SRC RCC_DCKCFGR2_SDMMC2SEL_48MHZ +#define STM32_RCC_DCKCFGR2_DSISRC RCC_DCKCFGR2_DSISEL_PHY + + +/* Several prescalers allow the configuration of the two AHB buses, the + * high-speed APB (APB2) and the low-speed APB (APB1) domains. The maximum + * frequency of the two AHB buses is 216 MHz while the maximum frequency of + * the high-speed APB domains is 108 MHz. The maximum allowed frequency of + * the low-speed APB domain is 54 MHz. + */ + +/* AHB clock (HCLK) is SYSCLK (216 MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (54 MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (108MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + + +/* FLASH wait states + * + * --------- ---------- ----------- + * VDD MAX SYSCLK WAIT STATES + * --------- ---------- ----------- + * 1.7-2.1 V 180 MHz 8 + * 2.1-2.4 V 216 MHz 9 + * 2.4-2.7 V 216 MHz 8 + * 2.7-3.6 V 216 MHz 7 + * --------- ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 7 + +/* LED definitions ******************************************************************/ +/* The board has numerous LEDs but 1, RED LED + + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red + * ---------------------- -------------------------- ------ */ + +#define LED_STARTED 0 /* NuttX has been started OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF */ +#define LED_INIRQ 4 /* In an interrupt N/C */ +#define LED_SIGNAL 5 /* In a signal handler N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + + +/* UARTs */ + +#define GPIO_USART2_TX GPIO_USART2_TX_1 /* PA2 */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_1 /* PB10 */ +#define GPIO_USART3_RX GPIO_USART3_RX_1 /* PB11 */ + + +/* CAN */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_2 /* PB8 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_2 /* PB9 */ + + +/* I2C */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 /* PA8 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 /* PC9 */ + + +/* SPI */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 /* PC11 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_3 /* PC12 */ +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_1 /* PB3 */ + diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..ca0d10189f --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 + +/* +| DMA1 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | SPDIFRX_DT | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | SPDIFRX_CS | SPI3_TX_2 | +| Channel 1 | I2C1_RX | I2C3_RX | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_RX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | - | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| Channel 8 | I2C3_TX | I2C4_RX | - | - | I2C2_TX | - | I2C4_TX | - | +| Channel 9 | - | SPI2_RX | - | - | - | - | SPI2_TX | - | +| | | | | | | | | | +| Usage | SPI3_RX_1 | | | | | USART2_RX | USART2_TX | SPI3_TX_2 | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_1 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | SAI2_B_2 | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | SAI2_A | SPI1_TX_2 | SAI2_B | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDMMC1_1 | - | USART1_RX_2 | SDMMC1_2 | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| Channel 8 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | +| Channel 9 | JPEG_IN | JPEG_OUT | SPI4_TX | JPEG_IN | JPEG_OUT | SPI5_RX | - | - | +| Channel 10 | SAI1_B | SAI2_B | SAI2_A | - | - | - | SAI1_A | - | +| Channel 11 | SDMMC2 | - | QUADSPI | - | - | SDMMC2 | - | - | +| | | | | | | | | | +| Usage | SPI4_RX_1 | TIM8_UP | | | SPI4_TX_2 | TIM1_UP | | | + */ + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// SPI3_RX_1 // DMA1, Stream 0, Channel 0 +// AVAILABLE // DMA1, Stream 1 +// AVAILABLE // DMA1, Stream 1 +// AVAILABLE // DMA1, Stream 3 +// AVAILABLE // DMA1, Stream 4 +// DMAMAP_USART2_RX // DMA1, Stream 5, Channel 4 +// DMAMAP_USART2_TX // DMA1, Stream 6, Channel 4 +// SPI3_TX_2 // DMA1, Stream 7, Channel 0 + + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// AVAILABLE // DMA1, Stream 0 +// AVAILABLE // DMA1, Stream 1 +#define DMAMAP_USART1_RX USART1_RX_1 // DMA1, Stream 2, Channel 4 +// AVAILABLE // DMA1, Stream 3 +// AVAILABLE // DMA1, Stream 4 +// AVAILABLE // DMA1, Stream 5 +// AVAILABLE // DMA1, Stream 6 +// DMAMAP_USART1_TX USART1_TX // DMA1, Stream 7, Channel 5 + diff --git a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..6c0573ef19 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,175 @@ +# +# 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_DISABLE_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME 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="stm32f7" +CONFIG_ARCH_CHIP_STM32F722RE=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTSTR="PX4 FreeFly RTK GPS" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="Freefly" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +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_INCLUDE_PROGMEM=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_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NFILE_DESCRIPTORS=12 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_MW=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=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_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=256 +CONFIG_STM32F7_CAN1=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_HSECLOCK=y +CONFIG_STM32F7_RTC_MAGIC=0xfacefeee +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_TIM8=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1100 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..f63cd44b6c --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,155 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F722ZE has 512 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F32RE, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option bytes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F32RE will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F32RE also has 256 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 64 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 176 KiB of SRAM1 beginning at address 0x2001:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2003:c000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00200000, LENGTH = 512K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + sram1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K + sram2 (rwx) : ORIGIN = 0x2003c000, LENGTH = 16K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .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(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram1 AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram1 + + /* 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/freefly/can-rtk-gps/nuttx-config/scripts/script.ld b/boards/freefly/can-rtk-gps/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..6e6424daa0 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/scripts/script.ld @@ -0,0 +1,169 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F32RE has 512 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F32RE, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option bytes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F32RE will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F32RE also has 256 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 64 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 176 KiB of SRAM1 beginning at address 0x2001:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2003:c000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00200000, LENGTH = 512K + flash (rx) : ORIGIN = 0x08010000, LENGTH = 512K-64K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + sram1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K + sram2 (rwx) : ORIGIN = 0x2003c000, LENGTH = 16K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.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 functions (static constructors and the like) + */ + .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(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram1 AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram1 + + /* 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/freefly/can-rtk-gps/src/CMakeLists.txt b/boards/freefly/can-rtk-gps/src/CMakeLists.txt new file mode 100644 index 0000000000..bb4ba28a59 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + i2c.cpp + led.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + spi.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + nuttx_arch + nuttx_drivers + px4_layer + arch_io_pins + ) +endif() diff --git a/boards/freefly/can-rtk-gps/src/board_config.h b/boards/freefly/can-rtk-gps/src/board_config.h new file mode 100644 index 0000000000..8a8bbc272a --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/board_config.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +/* BUTTON *************************************************************************** */ + +#define BUTTON_SAFETY /* PC14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN14|GPIO_EXTI) + +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN6) +#define GPIO_BTN_SAFETY (BUTTON_SAFETY & ~GPIO_EXTI) + + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 2 /* use timer 2 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define BOARD_ENABLE_CONSOLE_BUFFER + +__BEGIN_DECLS + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/freefly/can-rtk-gps/src/boot.c b/boards/freefly/can-rtk-gps/src/boot.c new file mode 100644 index 0000000000..ee1b1f6765 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/boot.c @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "led.h" +#include "board.h" + +#include +#include +#include + +#include + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 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 stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + + stm32_configgpio(GPIO_LED_SAFETY); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 10, 63, 31, 255), + led(2, autobaud_start, 0, 63, 0, 1), + led(3, autobaud_end, 0, 63, 0, 2), + led(4, allocation_start, 0, 0, 31, 2), + led(5, allocation_end, 0, 63, 31, 3), + led(6, fw_update_start, 15, 63, 31, 3), + led(7, fw_update_erase_fail, 15, 63, 15, 3), + led(8, fw_update_invalid_response, 31, 0, 0, 1), + led(9, fw_update_timeout, 31, 0, 0, 2), + led(a, fw_update_invalid_crc, 31, 0, 0, 4), + led(b, jump_to_app, 0, 63, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red << 3, + i2l[indication].green << 2, + i2l[indication].blue << 3, + i2l[indication].hz); +} + +__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) +{ +} diff --git a/boards/freefly/can-rtk-gps/src/boot_config.h b/boards/freefly/can-rtk-gps/src/boot_config.h new file mode 100644 index 0000000000..d5510142e7 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/boot_config.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 5000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* I2C LED needs 8 bytes */ + +#define OPT_SIMPLE_MALLOC_HEAP_SIZE 8 + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER GPIO_BTN_SAFETY diff --git a/boards/freefly/can-rtk-gps/src/can.c b/boards/freefly/can-rtk-gps/src/can.c new file mode 100644 index 0000000000..9e9d09c4f9 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/can.c @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * 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 can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include "board_config.h" + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/freefly/can-rtk-gps/src/i2c.cpp b/boards/freefly/can-rtk-gps/src/i2c.cpp new file mode 100644 index 0000000000..3ee9ff0b62 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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] = { + initI2CBusInternal(1), + initI2CBusInternal(3), +}; diff --git a/boards/freefly/can-rtk-gps/src/init.c b/boards/freefly/can-rtk-gps/src/init.c new file mode 100644 index 0000000000..49d67ac7ce --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/init.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * 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. + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include + +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 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 stm32_boardinitialize(void) +{ + //watchdog_init(); + + // Configure CAN interface + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + + stm32_configgpio(GPIO_LED_SAFETY); + stm32_configgpio(GPIO_BTN_SAFETY); + + // Configure SPI all interfaces GPIO & enable power. + stm32_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. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {1, 32 * 1024, 0x08008000}, + {2, 32 * 1024, 0x08010000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int 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); + return -ENODEV; + } + +#endif // FLASH_BASED_PARAMS + + //px4_platform_configure(); + + return OK; +} diff --git a/boards/freefly/can-rtk-gps/src/led.cpp b/boards/freefly/can-rtk-gps/src/led.cpp new file mode 100644 index 0000000000..838e6590c3 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/led.cpp @@ -0,0 +1,239 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * 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. + * + ****************************************************************************/ +#include + +#include +#include + +#include +#include "led.h" + +#include + +#include +#include +#include + +__BEGIN_DECLS +#include "cxx_init.h" +void board_autoled_on(int led); +void board_autoled_off(int led); +__END_DECLS + +using namespace time_literals; +#define MODULE_NAME "LED" + +#define ADDR 0x39 /**< I2C adress of NCP5623C */ + +#define NCP5623_LED_CURRENT 0x20 /**< Current register */ +#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ +#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ +#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ + +#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ +#define NCP5623_LED_OFF 0x00 /**< off */ + + +class RGBLED_NPC5623C : public device::I2C +{ +public: + RGBLED_NPC5623C(const int bus, int bus_frequency, const int address); + virtual ~RGBLED_NPC5623C() = default; + + int init() override; + int probe() override; + + int send_led_rgb(uint8_t red, uint8_t green, uint8_t blue); + + +private: + float _brightness{1.0f}; + float _max_brightness{1.0f}; + + int write(uint8_t reg, uint8_t data); +}; + +RGBLED_NPC5623C::RGBLED_NPC5623C(const int bus, int bus_frequency, const int address) : + I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency) +{ +} + +int +RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) +{ + uint8_t msg[1] = { 0x00 }; + msg[0] = ((reg & 0xe0) | (data & 0x1f)); + + int ret = transfer(&msg[0], 1, nullptr, 0); + + return ret; +} + +int +RGBLED_NPC5623C::init() +{ + int ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +RGBLED_NPC5623C::probe() +{ + _retries = 4; + + return write(NCP5623_LED_CURRENT, 0x00); +} + + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED_NPC5623C::send_led_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + + uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80}; + uint8_t brightness = 0x1f * _max_brightness; + + msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f); + msg[2] = NCP5623_LED_PWM0 | (uint8_t(red * _brightness) & 0x1f); + msg[4] = NCP5623_LED_PWM1 | (uint8_t(green * _brightness) & 0x1f); + msg[6] = NCP5623_LED_PWM2 | (uint8_t(blue * _brightness) & 0x1f); + + return transfer(&msg[0], 7, nullptr, 0); +} + +static RGBLED_NPC5623C instance(1, 100000, ADDR); + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +static uint8_t _rgb[] = {0, 0, 0}; + +static int timerInterrupt(int irq, void *context, void *arg) +{ + putreg16(~getreg16(TMR_REG(STM32_GTIM_SR_OFFSET)), TMR_REG(STM32_GTIM_SR_OFFSET)); + + static int d2 = 1; + (d2++ & 1) ? instance.send_led_rgb(0, 0, 0) : instance.send_led_rgb(_rgb[0], _rgb[1], _rgb[2]); + + return 0; +} + + +void rgb_led(int r, int g, int b, int freqs) +{ + long fosc = TMR_FREQUENCY; + long prescale = 1536; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = false; + + if (!once) { + cxx_initialize(); + + if (instance.init() != PX4_OK) { + return; + } + + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + putreg32(p0p5s + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + + irq_attach(STM32_IRQ_TIM1CC, timerInterrupt, NULL); + up_enable_irq(STM32_IRQ_TIM1CC); + putreg16(GTIM_DIER_CC1IE, TMR_REG(STM32_GTIM_DIER_OFFSET)); + once = true; + } + + long p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + putreg32(p + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); + putreg32(p, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + _rgb[0] = r; + _rgb[1] = g; + _rgb[2] = b; + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +__EXPORT void board_autoled_on(int led) +{ + if (led == LED_ASSERTION || led == LED_PANIC) { + stm32_gpiowrite(GPIO_LED_SAFETY, true); + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +__EXPORT void board_autoled_off(int led) +{ + if (led == LED_ASSERTION || led == LED_PANIC) { + stm32_gpiowrite(GPIO_LED_SAFETY, false); + } + +} diff --git a/boards/freefly/can-rtk-gps/src/led.h b/boards/freefly/can-rtk-gps/src/led.h new file mode 100644 index 0000000000..b68e4aa70d --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/freefly/can-rtk-gps/src/spi.cpp b/boards/freefly/can-rtk-gps/src/spi.cpp new file mode 100644 index 0000000000..cf00932464 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/spi.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * 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::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG, SPI::CS{GPIO::PortA, GPIO::Pin15}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/freefly/can-rtk-gps/src/usb.c b/boards/freefly/can-rtk-gps/src/usb.c new file mode 100644 index 0000000000..587516cb8d --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/usb.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef GPIO_OTGFS_VBUS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * 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 stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/freefly/can-rtk-gps/uavcan_board_identity b/boards/freefly/can-rtk-gps/uavcan_board_identity new file mode 100644 index 0000000000..997b26e2ae --- /dev/null +++ b/boards/freefly/can-rtk-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 85) +set(uavcanblid_name "\"org.freefly.can-rtk-gps\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 62fdbe3c57..7aafa33770 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -72,6 +72,7 @@ elseif("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") -Wl,-wrap,nxsched_process_timer -Wl,-wrap,nxsem_post -Wl,-wrap,nxsem_wait + -Wl,-wrap,nxsem_wait_uninterruptible -Wl,-wrap,arm_svcall -Wl,-wrap,nx_start -Wl,-wrap,exit @@ -248,6 +249,9 @@ if(NOT NUTTX_DIR MATCHES "external") elseif(CONFIG_ARCH_CHIP_STM32F469I) set(DEBUG_DEVICE "STM32F469II") set(DEBUG_SVD_FILE "STM32F469.svd") + elseif(CONFIG_ARCH_CHIP_STM32F722RE) + set(DEBUG_DEVICE "STM32F722RE") + set(DEBUG_SVD_FILE "STM32F7x2.svd") elseif(CONFIG_ARCH_CHIP_STM32F745VG) set(DEBUG_DEVICE "STM32F745VG") set(DEBUG_SVD_FILE "STM32F7x5.svd") diff --git a/platforms/nuttx/src/canbootloader/CMakeLists.txt b/platforms/nuttx/src/canbootloader/CMakeLists.txt index be91358d05..8ca45a1e87 100644 --- a/platforms/nuttx/src/canbootloader/CMakeLists.txt +++ b/platforms/nuttx/src/canbootloader/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_library(canbootloader sched/timer.c uavcan/main.c util/random.c + util/cxx_init.c ) include_directories(include) diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt new file mode 100644 index 0000000000..eccd131c6d --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(../../../include) + +px4_add_library(arch_canbootloader + board_identity.c + drivers/can/driver.c +) + +target_link_libraries(arch_canbootloader + PRIVATE + arch_watchdog_iwdg +) diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c new file mode 100644 index 0000000000..a716ad2f12 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_identity.c + * Implementation of STM32 based Board identity API + */ + +#include +#include +#include + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +int board_get_mfguid(mfguid_t mfgid) +{ + uint32_t *chip_uuid = (uint32_t *) STM32_SYSMEM_UID; + uint32_t *rv = (uint32_t *) &mfgid[0]; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + *rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + } + + return PX4_CPU_MFGUID_BYTE_LENGTH; +} diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c new file mode 100644 index 0000000000..5a90ea708f --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c @@ -0,0 +1,566 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * 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. + * + ****************************************************************************/ + +#include +#include "boot_config.h" + +#include +#include +#include +#include +#include + +#include "chip.h" +#include +#include "nvic.h" + +#include "board.h" +#include +#include "can.h" +#include "timer.h" + +#include + +#include + +#define INAK_TIMEOUT 65535 + +#define CAN_TX_TIMEOUT_MS (200 /(1000/(1000000/CONFIG_USEC_PER_TICK))) + +#define SJW_POS 24 +#define BS1_POS 16 +#define BS2_POS 20 + +#define CAN_TSR_RQCP_SHFTS 8 + +#define FILTER_ID 1 +#define FILTER_MASK 2 + +#if STM32_PCLK1_FREQUENCY == 54000000 +/* Sample 85.7 % */ +# define QUANTA 18 +# define BS1_VALUE 14 +# define BS2_VALUE 1 +#elif STM32_PCLK1_FREQUENCY == 48000000 +/* Sample 85.7 % */ +# define QUANTA 16 +# define BS1_VALUE 12 +# define BS2_VALUE 1 +#elif STM32_PCLK1_FREQUENCY == 45000000 || STM32_PCLK1_FREQUENCY == 36000000 +/* Sample 88.9 % */ +# define QUANTA 9 +# define BS1_VALUE 6 +# define BS2_VALUE 0 +#elif STM32_PCLK1_FREQUENCY == 42000000 +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#else +# warning Undefined QUANTA bsed on Clock Rate +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#endif + +#define CAN_1MBAUD_SJW 0 +#define CAN_1MBAUD_BS1 BS1_VALUE +#define CAN_1MBAUD_BS2 BS2_VALUE +#define CAN_1MBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/1000000/QUANTA) + +#define CAN_500KBAUD_SJW 0 +#define CAN_500KBAUD_BS1 BS1_VALUE +#define CAN_500KBAUD_BS2 BS2_VALUE +#define CAN_500KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/500000/QUANTA) + +#define CAN_250KBAUD_SJW 0 +#define CAN_250KBAUD_BS1 BS1_VALUE +#define CAN_250KBAUD_BS2 BS2_VALUE +#define CAN_250KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/250000/QUANTA) + +#define CAN_125KBAUD_SJW 0 +#define CAN_125KBAUD_BS1 BS1_VALUE +#define CAN_125KBAUD_BS2 BS2_VALUE +#define CAN_125KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/125000/QUANTA) + +#define CAN_BTR_LBK_SHIFT 30 + +// Number of CPU cycles for a single bit time at the supported speeds +#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US)) + +#define CAN_BAUD_TIME_IN_MS 200 +#define CAN_BAUD_SAMPLES_NEEDED 32 +#define CAN_BAUD_SAMPLES_DISCARDED 8 + +static inline uint32_t read_msr_rx(void) +{ + return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX; +} + +static uint32_t read_msr(time_hrt_cycles_t *now) +{ + __asm__ __volatile__("\tcpsid i\n"); + *now = timer_hrt_read(); + uint32_t msr = read_msr_rx(); + __asm__ __volatile__("\tcpsie i\n"); + return msr; +} + +static int read_bits_times(time_hrt_cycles_t *times, size_t max) +{ + uint32_t samplecnt = 0; + bl_timer_id ab_timer = timer_allocate(modeTimeout | modeStarted, CAN_BAUD_TIME_IN_MS, 0); + time_ref_t ab_ref = timer_ref(ab_timer); + uint32_t msr; + uint32_t last_msr = read_msr(times); + + while (samplecnt < max && !timer_ref_expired(ab_ref)) { + do { + msr = read_msr(×[samplecnt]); + } while (!(msr ^ last_msr) && !timer_ref_expired(ab_ref)); + + last_msr = msr; + samplecnt++; + } + + timer_free(ab_timer); + return samplecnt; +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a can_speed_t to a bit rate in Hz + * + * Input Parameters: + * can_speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + * Returned value: + * Bit rate in Hz + * + ****************************************************************************/ +int can_speed2freq(can_speed_t speed) + +{ + return 1000000 >> (CAN_1MBAUD - speed); +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a frequency in Hz to a can_speed_t in the range + * CAN_125KBAUD to CAN_1MBAUD. + * + * Input Parameters: + * freq - Bit rate in Hz + * + * Returned value: + * A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + ****************************************************************************/ +can_speed_t can_freq2speed(int freq) +{ + return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); +} + +/**************************************************************************** + * Name: can_tx + * + * Description: + * This function is called to transmit a CAN frame using the supplied + * mailbox. It will busy wait on the mailbox if not available. + * + * Input Parameters: + * message_id - The CAN message's EXID field + * length - The number of bytes of data - the DLC field + * message - A pointer to 8 bytes of data to be sent (all 8 bytes will be + * loaded into the CAN transmitter but only length bytes will + * be sent. + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * The CAN_OK of the data sent or CAN_ERROR if a time out occurred + * + ****************************************************************************/ +uint8_t can_tx(uint32_t message_id, size_t length, const uint8_t *message, uint8_t mailbox) +{ + uint32_t data[2]; + + memcpy(data, message, sizeof(data)); + + /* + * Just block while waiting for the mailbox. + */ + + uint32_t mask = CAN_TSR_TME0 << mailbox; + + /* Reset the indication of timer expired */ + + timer_hrt_clear_wrap(); + uint32_t cnt = CAN_TX_TIMEOUT_MS; + + while ((getreg32(STM32_CAN1_TSR) & mask) == 0) { + if (timer_hrt_wrap()) { + timer_hrt_clear_wrap(); + + if (--cnt == 0) { + return CAN_ERROR; + } + } + } + + /* + * To allow detection of completion - Set the LEC to + * 'No error' state off all 1s + */ + + putreg32(CAN_ESR_LEC_MASK, STM32_CAN1_ESR); + + putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox)); + putreg32(data[0], STM32_CAN1_TDLR(mailbox)); + putreg32(data[1], STM32_CAN1_TDHR(mailbox)); + putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ, + STM32_CAN1_TIR(mailbox)); + return CAN_OK; +} + +/**************************************************************************** + * Name: can_rx + * + * Description: + * This function is called to receive a CAN frame from a supplied fifo. + * It does not block if there is not available, but returns 0 + * + * Input Parameters: + * message_id - A pointer to return the CAN message's EXID field + * length - A pointer to return the number of bytes of data - the DLC field + * message - A pointer to return 8 bytes of data to be sent (all 8 bytes will + * be written from the CAN receiver but only length bytes will be sent. + * fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo. + * + * Returned value: + * The length of the data read or 0 if the fifo was empty + * + ****************************************************************************/ +uint8_t can_rx(uint32_t *message_id, size_t *length, uint8_t *message, uint8_t fifo) +{ + uint32_t data[2]; + uint8_t rv = 0; + const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R }; + + if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK) { + + rv = 1; + /* If so, process it */ + + *message_id = (getreg32(STM32_CAN1_RIR(fifo)) & CAN_RIR_EXID_MASK) >> + CAN_RIR_EXID_SHIFT; + *length = (getreg32(STM32_CAN1_RDTR(fifo)) & CAN_RDTR_DLC_MASK) >> + CAN_RDTR_DLC_SHIFT; + data[0] = getreg32(STM32_CAN1_RDLR(fifo)); + data[1] = getreg32(STM32_CAN1_RDHR(fifo)); + + putreg32(CAN_RFR_RFOM, fifos[fifo & 1]); + + memcpy(message, data, sizeof(data)); + } + + return rv; +} + +/**************************************************************************** + * Name: can_autobaud + * + * Description: + * This function will attempt to detect the bit rate in use on the CAN + * interface until the timeout provided expires or the successful detection + * occurs. + * + * It will initialize the CAN block for a given bit rate + * to test that a message can be received. The CAN interface is left + * operating at the detected bit rate and in CAN_Mode_Normal mode. + * + * Input Parameters: + * can_speed - A pointer to return detected can_speed_t from CAN_UNKNOWN to + * CAN_1MBAUD + * timeout - The timer id of a timer to use as the maximum time to wait for + * successful bit rate detection. This timer may be not running + * in which case the auto baud code will try indefinitely to + * detect the bit rate. + * + * Returned value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) +{ + *can_speed = CAN_UNKNOWN; + + volatile int attempt = 0; + /* Threshold are at 1.5 Bit times */ + + /* + * We are here because there was a reset or the app invoked + * the bootloader with no bit rate set. + */ + + time_hrt_cycles_t bit_time; + time_hrt_cycles_t min_cycles; + int sample; + can_speed_t speed = CAN_125KBAUD; + + time_hrt_cycles_t samples[128]; + + while (1) { + + + while (1) { + + min_cycles = ULONG_MAX; + int samplecnt = read_bits_times(samples, arraySize(samples)); + + if (timer_expired(timeout)) { + return CAN_BOOT_TIMEOUT; + } + + if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) & + CAN_RFR_FMP_MASK) { + *can_speed = speed; + can_init(speed, CAN_Mode_Normal); + return CAN_OK; + } + + if (samplecnt < CAN_BAUD_SAMPLES_NEEDED) { + continue; + } + + for (sample = 0; sample < samplecnt; sample += 2) { + bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample + 1]); + + if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) { + min_cycles = bit_time; + } + } + + break; + } + + uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES / 4; + samples[1] = min_cycles; + speed = CAN_125KBAUD; + + while (min_cycles < bit34 && speed < CAN_1MBAUD) { + speed++; + bit34 /= 2; + } + + attempt++; + can_init(speed, CAN_Mode_Silent); + + + } /* while(1) */ + + return CAN_OK; +} + +/**************************************************************************** + * Name: can_init + * + * Description: + * This function is used to initialize the CAN block for a given bit rate and + * mode. + * + * Input Parameters: + * speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * mode - One of the can_mode_t of Normal, LoopBack and Silent or + * combination thereof. + * + * Returned value: + * OK - on Success or a negate errno value + * + ****************************************************************************/ +int can_init(can_speed_t speed, can_mode_t mode) +{ + int speedndx = speed - 1; + /* + * TODO: use full-word writes to reduce the number of loads/stores. + * + * Also consider filter use -- maybe set filters for all the message types we + * want. */ + const uint32_t bitrates[] = { + (CAN_125KBAUD_SJW << SJW_POS) | + (CAN_125KBAUD_BS1 << BS1_POS) | + (CAN_125KBAUD_BS2 << BS2_POS) | (CAN_125KBAUD_PRESCALER - 1), + + (CAN_250KBAUD_SJW << SJW_POS) | + (CAN_250KBAUD_BS1 << BS1_POS) | + (CAN_250KBAUD_BS2 << BS2_POS) | (CAN_250KBAUD_PRESCALER - 1), + + (CAN_500KBAUD_SJW << SJW_POS) | + (CAN_500KBAUD_BS1 << BS1_POS) | + (CAN_500KBAUD_BS2 << BS2_POS) | (CAN_500KBAUD_PRESCALER - 1), + + (CAN_1MBAUD_SJW << SJW_POS) | + (CAN_1MBAUD_BS1 << BS1_POS) | + (CAN_1MBAUD_BS2 << BS2_POS) | (CAN_1MBAUD_PRESCALER - 1) + }; + + /* Remove Unknow Offset */ + if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) { + return -EINVAL; + } + + uint32_t timeout; + /* + * Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP + * knock down Sleep and raise CAN_MCR_INRQ + */ + + putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR); + + /* Wait until initialization mode is acknowledged */ + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) { + /* We are in initialization mode */ + + break; + } + } + + if (timeout < 1) { + /* + * Initialization failed, not much we can do now other than try a normal + * startup. */ + return -ETIME; + } + + + putreg32(bitrates[speedndx] | mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR); + putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF | CAN_MCR_TXFP, STM32_CAN1_MCR); + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) { + /* We are in initialization mode */ + + break; + } + } + + if (timeout < 1) { + return -ETIME; + } + + /* + * CAN filter initialization -- accept everything on RX FIFO 0, and only + * GetNodeInfo requests on RX FIFO 1. */ + + /* Set FINIT - Initialization mode for the filters- */ + putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); + + putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ + + putreg32(3, STM32_CAN1_FS1R); /* Single 32-bit scale configuration for filters 0 and 1 */ + + /* Filter 0 masks -- DTIDGetNodeInfo requests only */ + + uavcan_protocol_t protocol; + + protocol.id.u32 = 0; + protocol.ser.type_id = DTIDReqGetNodeInfo; + protocol.ser.service_not_message = true; + protocol.ser.request_not_response = true; + + /* Set the Filter ID */ + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_ID)); + + /* Set the Filter Mask */ + protocol.ser.type_id = BIT_MASK(LengthUavCanServiceTypeID); + + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_MASK)); + + /* Filter 1 masks -- everything is don't-care */ + putreg32(0, STM32_CAN1_FIR(1, FILTER_ID)); + putreg32(0, STM32_CAN1_FIR(1, FILTER_MASK)); + + putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */ + putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the + * rest of filters */ + putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */ + + /* Clear FINIT - Active mode for the filters- */ + + putreg32(0, STM32_CAN1_FMR); + + return OK; +} + +/**************************************************************************** + * Name: can_cancel_on_error + * + * Description: + * This function will test for transition completion or any error. + * If the is a error the the transmit will be aborted. + * + * Input Parameters: + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * CAN_OK - on Success or a CAN_ERROR if the cancellation was needed + * + ****************************************************************************/ +void can_cancel_on_error(uint8_t mailbox) +{ + uint32_t rvalue; + + /* Wait for completion the all 1's wat set in the tx code*/ + while (CAN_ESR_LEC_MASK == ((rvalue = (getreg32(STM32_CAN1_ESR) & CAN_ESR_LEC_MASK)))); + + /* Any errors */ + if (rvalue) { + + putreg32(0, STM32_CAN1_ESR); + + /* Abort the the TX in case of collision wuth NART false */ + + putreg32(CAN_TSR_ABRQ0 << (mailbox * CAN_TSR_RQCP_SHFTS), STM32_CAN1_TSR); + } +} diff --git a/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c b/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c index 897bca8718..a1ef574fce 100644 --- a/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c +++ b/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c @@ -40,6 +40,7 @@ #include "nuttx/arch.h" #include "arm_internal.h" +#include "boot_config.h" /**************************************************************************** * Pre-processor Definitions @@ -81,6 +82,12 @@ int main(int argc, char **argv); * ****************************************************************************/ +int __wrap_nxsem_wait_uninterruptible(void *sem) +{ + return 0; + +} + int __wrap_nxsem_wait(void *sem) { return 0; @@ -174,13 +181,27 @@ void __wrap_nx_start(void) * Name: malloc * * Description: - * This function hijacks the OS's malloc and provides no allocation + * This function hijacks the OS's malloc and provides no or small + * allocation * ****************************************************************************/ FAR void *malloc(size_t size) { +#if defined(OPT_SIMPLE_MALLOC_HEAP_SIZE) + static int aloc = 0; + static uint8_t mem[OPT_SIMPLE_MALLOC_HEAP_SIZE]; + void *rv = &mem[aloc]; + aloc += size; + + if (aloc > OPT_SIMPLE_MALLOC_HEAP_SIZE) { + PANIC(); + } + + return rv; +#else return NULL; +#endif } /**************************************************************************** diff --git a/platforms/nuttx/src/canbootloader/include/cxx_init.h b/platforms/nuttx/src/canbootloader/include/cxx_init.h new file mode 100644 index 0000000000..45feec8417 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/cxx_init.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * Name: cxx_initialize + * + * Description: + * This function initialises static C++N objects + * + * + * Input Parameters: + * None + * + * Returned value: + * None + * + ****************************************************************************/ +void cxx_initialize(void); diff --git a/platforms/nuttx/src/canbootloader/util/cxx_init.c b/platforms/nuttx/src/canbootloader/util/cxx_init.c new file mode 100644 index 0000000000..fe47063564 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/util/cxx_init.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#ifdef CONFIG_HAVE_CXXINITIALIZE +#include "cxx_init.h" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This type defines one entry in initialization array */ + +typedef CODE void (*initializer_t)(void); + +/**************************************************************************** + * External References + ****************************************************************************/ + +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uintptr_t _stext; +extern uintptr_t _etext; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cxx_initialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; this + * function definition only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support. + * + ****************************************************************************/ + +void cxx_initialize(void) +{ + static int inited = 0; + + if (inited == 0) { + initializer_t *initp; + + sinfo("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialization table */ + + for (initp = &_sinit; initp != &_einit; initp++) { + initializer_t initializer = *initp; + sinfo("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text + * region defined by the linker script. Some toolchains may put + * NULL values or counts in the initialization table. + */ + + if ((FAR void *)initializer >= (FAR void *)&_stext && + (FAR void *)initializer < (FAR void *)&_etext) { + sinfo("Calling %p\n", initializer); + initializer(); + } + } + + inited = 1; + } +} + +#endif diff --git a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt index 48a1aee444..b55e895db7 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt @@ -45,4 +45,4 @@ add_subdirectory(../stm32_common/tone_alarm tone_alarm) add_subdirectory(../stm32_common/version version) add_subdirectory(px4io_serial) - +add_subdirectory(watchdog) diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h index 4652afe349..540c09a646 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h @@ -39,11 +39,15 @@ __BEGIN_DECLS #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F7 #include +#include +#include #include #include //include up_systemreset() which is included on stm32.h -#include -#define PX4_BBSRAM_SIZE STM32F7_BBSRAM_SIZE -#define PX4_BBSRAM_GETDESC_IOCTL STM32F7_BBSRAM_GETDESC_IOCTL +#if defined(CONFIG_STM32F7_BKPSRAM) +# include +# define PX4_BBSRAM_SIZE STM32F7_BBSRAM_SIZE +# define PX4_BBSRAM_GETDESC_IOCTL STM32F7_BBSRAM_GETDESC_IOCTL +#endif // CONFIG_STM32F7_BKPSRAM #define PX4_FLASH_BASE 0x08000000 #define PX4_NUMBER_I2C_BUSES STM32F7_NI2C #define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 18 diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt new file mode 100644 index 0000000000..3c80878d02 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/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_watchdog_iwdg + iwdg.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c new file mode 100644 index 0000000000..a2c272a385 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * 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. + * + ****************************************************************************/ + +#include +#include "arm_internal.h" +#include "arm_arch.h" +#include "chip.h" + +#include "nvic.h" + +#include "stm32_wdg.h" + +/**************************************************************************** + * Name: watchdog_pet() + * + * Description: + * This function resets the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_pet(void) +{ + putreg32(IWDG_KR_KEY_RELOAD, STM32_IWDG_KR); +} + +/**************************************************************************** + * Name: watchdog_init() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_init(void) +{ +#if defined(CONFIG_STM32F7_JTAG_FULL_ENABLE) || \ + defined(CONFIG_STM32F7_JTAG_NOJNTRST_ENABLE) || \ + defined(CONFIG_STM32F7_JTAG_SW_ENABLE) + putreg32(getreg32(STM32_DBGMCU_APB1_FZ) | DBGMCU_APB1_IWDGSTOP, STM32_DBGMCU_APB1_FZ); +#endif + + /* unlock */ + + putreg32(IWDG_KR_KEY_ENABLE, STM32_IWDG_KR); + + /* Set the prescale value */ + + putreg32(IWDG_PR_DIV16, STM32_IWDG_PR); + + /* Set the reload value */ + + putreg32(IWDG_RLR_MAX, STM32_IWDG_RLR); + + /* Start the watch dog */ + + putreg32(IWDG_KR_KEY_START, STM32_IWDG_KR); + + watchdog_pet(); +} diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h new file mode 100644 index 0000000000..9150b841be --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h @@ -0,0 +1,162 @@ +/************************************************************************************ + * arch/arm/src/stm32/hardware/stm32_wdg.h + * + * Copyright (C) 2009, 2011-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H +#define __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include "chip.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Register Offsets *****************************************************************/ + +#define STM32_IWDG_KR_OFFSET 0x0000 /* Key register (32-bit) */ +#define STM32_IWDG_PR_OFFSET 0x0004 /* Prescaler register (32-bit) */ +#define STM32_IWDG_RLR_OFFSET 0x0008 /* Reload register (32-bit) */ +#define STM32_IWDG_SR_OFFSET 0x000c /* Status register (32-bit) */ +#if defined(CONFIG_STM32_STM32F30XX) +# define STM32_IWDG_WINR_OFFSET 0x000c /* Window register (32-bit) */ +#endif + +#define STM32_WWDG_CR_OFFSET 0x0000 /* Control Register (32-bit) */ +#define STM32_WWDG_CFR_OFFSET 0x0004 /* Configuration register (32-bit) */ +#define STM32_WWDG_SR_OFFSET 0x0008 /* Status register (32-bit) */ + +/* Register Addresses ***************************************************************/ + +#define STM32_IWDG_KR (STM32_IWDG_BASE+STM32_IWDG_KR_OFFSET) +#define STM32_IWDG_PR (STM32_IWDG_BASE+STM32_IWDG_PR_OFFSET) +#define STM32_IWDG_RLR (STM32_IWDG_BASE+STM32_IWDG_RLR_OFFSET) +#define STM32_IWDG_SR (STM32_IWDG_BASE+STM32_IWDG_SR_OFFSET) +#if defined(CONFIG_STM32_STM32F30XX) +# define STM32_IWDG_WINR (STM32_IWDG_BASE+STM32_IWDG_WINR_OFFSET) +#endif + +#define STM32_WWDG_CR (STM32_WWDG_BASE+STM32_WWDG_CR_OFFSET) +#define STM32_WWDG_CFR (STM32_WWDG_BASE+STM32_WWDG_CFR_OFFSET) +#define STM32_WWDG_SR (STM32_WWDG_BASE+STM32_WWDG_SR_OFFSET) + +/* Register Bitfield Definitions ****************************************************/ + +/* Key register (32-bit) */ + +#define IWDG_KR_KEY_SHIFT (0) /* Bits 15-0: Key value (write only, read 0000h) */ +#define IWDG_KR_KEY_MASK (0xffff << IWDG_KR_KEY_SHIFT) + +#define IWDG_KR_KEY_ENABLE (0x5555) /* Enable register access */ +#define IWDG_KR_KEY_DISABLE (0x0000) /* Disable register access */ +#define IWDG_KR_KEY_RELOAD (0xaaaa) /* Reload the counter */ +#define IWDG_KR_KEY_START (0xcccc) /* Start the watchdog */ + +/* Prescaler register (32-bit) */ + +#define IWDG_PR_SHIFT (0) /* Bits 2-0: Prescaler divider */ +#define IWDG_PR_MASK (7 << IWDG_PR_SHIFT) +# define IWDG_PR_DIV4 (0 << IWDG_PR_SHIFT) /* 000: divider /4 */ +# define IWDG_PR_DIV8 (1 << IWDG_PR_SHIFT) /* 001: divider /8 */ +# define IWDG_PR_DIV16 (2 << IWDG_PR_SHIFT) /* 010: divider /16 */ +# define IWDG_PR_DIV32 (3 << IWDG_PR_SHIFT) /* 011: divider /32 */ +# define IWDG_PR_DIV64 (4 << IWDG_PR_SHIFT) /* 100: divider /64 */ +# define IWDG_PR_DIV128 (5 << IWDG_PR_SHIFT) /* 101: divider /128 */ +# define IWDG_PR_DIV256 (6 << IWDG_PR_SHIFT) /* 11x: divider /256 */ + +/* Reload register (32-bit) */ + +#define IWDG_RLR_RL_SHIFT (0) /* Bits11:0 RL[11:0]: Watchdog counter reload value */ +#define IWDG_RLR_RL_MASK (0x0fff << IWDG_RLR_RL_SHIFT) + +#define IWDG_RLR_MAX (0xfff) + +/* Status register (32-bit) */ + +#define IWDG_SR_PVU (1 << 0) /* Bit 0: Watchdog prescaler value update */ +#define IWDG_SR_RVU (1 << 1) /* Bit 1: Watchdog counter reload value update */ + +#if defined(CONFIG_STM32_STM32F30XX) +# define IWDG_SR_WVU (1 << 2) /* Bit 2: */ +#endif + +/* Window register (32-bit) */ + +#if defined(CONFIG_STM32_STM32F30XX) +# define IWDG_WINR_SHIFT (0) +# define IWDG_WINR_MASK (0x0fff << IWDG_WINR_SHIFT) +#endif + +/* Control Register (32-bit) */ + +#define WWDG_CR_T_SHIFT (0) /* Bits 6:0 T[6:0]: 7-bit counter (MSB to LSB) */ +#define WWDG_CR_T_MASK (0x7f << WWDG_CR_T_SHIFT) +# define WWDG_CR_T_MAX (0x3f << WWDG_CR_T_SHIFT) +# define WWDG_CR_T_RESET (0x40 << WWDG_CR_T_SHIFT) +#define WWDG_CR_WDGA (1 << 7) /* Bit 7: Activation bit */ + +/* Configuration register (32-bit) */ + +#define WWDG_CFR_W_SHIFT (0) /* Bits 6:0 W[6:0] 7-bit window value */ +#define WWDG_CFR_W_MASK (0x7f << WWDG_CFR_W_SHIFT) +#define WWDG_CFR_WDGTB_SHIFT (7) /* Bits 8:7 [1:0]: Timer Base */ +#define WWDG_CFR_WDGTB_MASK (3 << WWDG_CFR_WDGTB_SHIFT) +# define WWDG_CFR_PCLK1 (0 << WWDG_CFR_WDGTB_SHIFT) /* 00: CK Counter Clock (PCLK1 div 4096) div 1 */ +# define WWDG_CFR_PCLK1d2 (1 << WWDG_CFR_WDGTB_SHIFT) /* 01: CK Counter Clock (PCLK1 div 4096) div 2 */ +# define WWDG_CFR_PCLK1d4 (2 << WWDG_CFR_WDGTB_SHIFT) /* 10: CK Counter Clock (PCLK1 div 4096) div 4 */ +# define WWDG_CFR_PCLK1d8 (3 << WWDG_CFR_WDGTB_SHIFT) /* 11: CK Counter Clock (PCLK1 div 4096) div 8 */ +#define WWDG_CFR_EWI (1 << 9) /* Bit 9: Early Wakeup Interrupt */ + +/* Status register (32-bit) */ + +#define WWDG_SR_EWIF (1 << 0) /* Bit 0: Early Wakeup Interrupt Flag */ + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#endif /* __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H */