diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index dfd7924da6..f10e79c43d 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -64,6 +64,7 @@ pipeline { checkStatus() quickCalibrate() sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "calib_udelay"' } } stage("print topics") { diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin index e563493387..a6829bca73 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin differ diff --git a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig index 42c5602dd0..923ca26681 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig @@ -25,7 +25,7 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_INITTHREAD_PRIORITY=254 CONFIG_BOARD_LATE_INITIALIZE=y -CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_LOOPSPERMSEC=79954 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index 1b877718e0..3a4abd0235 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -60,7 +60,6 @@ #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. * @@ -82,7 +81,6 @@ * PLLP2,3 = {2, 3, 4, ..., 128} * CPUCLK <= 480 MHz */ -#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR * @@ -108,12 +106,12 @@ #define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) #define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) #define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(30) -#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(4) #define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(5) #define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(1) #define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 30) -#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 4) #define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 5) #define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 1) @@ -186,36 +184,28 @@ /* Kernel Clock Configuration * Note: look at Table 54 in ST Manual */ +#define STM32_RCC_D1CCIPR_SDMMCSEL RCC_D1CCIPR_SDMMC_PLL1 + #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ #define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ -#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ -#define STM32_FDCANCLK STM32_HSE_FREQUENCY +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 /* SDMMC definitions ********************************************************/ /* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ -#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#define STM32_SDMMC_INIT_CLKDIV (125 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) -/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) - * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 100 / (2*25) */ -#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) -# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) -#else -# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) -#endif -#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) -# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) -#else -# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) -#endif +#define STM32_SDMMC_MMCXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#define STM32_SDMMC_SDXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) #define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE diff --git a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig index d585050335..ac2df7b3eb 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig @@ -42,7 +42,7 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_LOOPSPERMSEC=79954 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig index 401e961cfa..7bea4b2425 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig @@ -41,7 +41,7 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_LOOPSPERMSEC=79954 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y @@ -52,7 +52,6 @@ CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x2DAE CONFIG_CDCACM_VENDORSTR="CubePilot" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -60,6 +59,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -82,7 +82,6 @@ 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 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp index d3a7c25439..92720faa24 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp @@ -263,7 +263,11 @@ int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings &out /* * Hardware configuration */ +#ifdef STM32_FDCANCLK const uavcan::uint32_t pclk = STM32_FDCANCLK; +#else + const uavcan::uint32_t pclk = STM32_HSE_FREQUENCY; +#endif static const int MaxBS1 = 16; static const int MaxBS2 = 8;