From 7c2da8d1ef1c2deb1097fce99247dd4380b0f679 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Mon, 17 Oct 2022 19:15:30 -0600 Subject: [PATCH] Enable ARKV6X SPIX SYNC, enable icm426889 and iim42652 CLKIN --- boards/ark/fmu-v6x/init/rc.board_sensors | 12 +- boards/ark/fmu-v6x/src/CMakeLists.txt | 7 + boards/ark/fmu-v6x/src/board_config.h | 11 +- boards/ark/fmu-v6x/src/init.c | 5 + boards/ark/fmu-v6x/src/spix_sync.c | 311 ++++++++++++++++++ boards/ark/fmu-v6x/src/spix_sync.h | 42 +++ boards/ark/fmu-v6x/src/timer_config.cpp | 16 +- .../px4_work_queue/WorkQueueManager.hpp | 14 +- .../stm32_common/include/px4_arch/io_timer.h | 7 + .../imu/invensense/icm42688p/CMakeLists.txt | 3 +- .../imu/invensense/icm42688p/ICM42688P.cpp | 59 +++- .../imu/invensense/icm42688p/ICM42688P.hpp | 17 +- .../InvenSense_ICM42688P_registers.hpp | 25 +- .../invensense/icm42688p/icm42688p_main.cpp | 9 +- .../imu/invensense/iim42652/CMakeLists.txt | 1 + .../imu/invensense/iim42652/IIM42652.cpp | 101 ++++-- .../imu/invensense/iim42652/IIM42652.hpp | 27 +- .../InvenSense_IIM42652_registers.hpp | 41 ++- .../imu/invensense/iim42652/iim42652_main.cpp | 9 +- 19 files changed, 648 insertions(+), 69 deletions(-) create mode 100644 boards/ark/fmu-v6x/src/spix_sync.c create mode 100644 boards/ark/fmu-v6x/src/spix_sync.h diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index 675457f812..84f985cecb 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -47,14 +47,14 @@ then fi fi -# Internal SPI bus IIM42652 -iim42652 -R 3 -s -b 1 start +# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz +iim42652 -R 3 -s -b 1 -C 32051 start -# Internal SPI bus ICM42688p -icm42688p -R 9 -s -b 2 start +# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz +icm42688p -R 9 -s -b 2 -C 32051 start -# Internal SPI bus ICM42688p -icm42688p -R 6 -s -b 3 start +# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz +icm42688p -R 6 -s -b 3 -C 32051 start # Internal magnetometer on I2C bmm150 -I start diff --git a/boards/ark/fmu-v6x/src/CMakeLists.txt b/boards/ark/fmu-v6x/src/CMakeLists.txt index c84d6f291c..3135751c66 100644 --- a/boards/ark/fmu-v6x/src/CMakeLists.txt +++ b/boards/ark/fmu-v6x/src/CMakeLists.txt @@ -31,14 +31,19 @@ # ############################################################################ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_compile_definitions(BOOTLOADER) add_library(drivers_board bootloader_main.c + init.c usb.c + timer_config.cpp ) target_link_libraries(drivers_board PRIVATE nuttx_arch # sdio nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer bootloader ) target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) @@ -53,6 +58,8 @@ else() manifest.c sdio.c spi.cpp + spix_sync.c + spix_sync.h timer_config.cpp usb.c ) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 89500da23f..e8b7b93854 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -125,7 +125,9 @@ /* SPI */ #define SPI6_nRESET_EXTERNAL1 /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10) -#define GPIO_SYNC /* PE9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_100MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_SYNC /* PE9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_SYNC_TIMER /* PE9 */ (GPIO_ALT|GPIO_AF1|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) + /* I2C busses */ @@ -251,7 +253,8 @@ /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 9 +#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define BOARD_PWM_FREQ 1024000 /* Power supply control and monitoring GPIOs */ @@ -458,7 +461,6 @@ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_VDD_3V3_SD_CARD_EN, \ GPIO_PD15, \ - GPIO_SYNC, \ SPI6_nRESET_EXTERNAL1, \ GPIO_ETH_POWER_EN, \ GPIO_NFC_GPIO, \ @@ -474,7 +476,8 @@ #define PX4_I2C_BUS_MTD 4,5 -#define BOARD_NUM_IO_TIMERS 5 +#define BOARD_NUM_IO_TIMERS 3 +#define BOARD_SPIX_SYNC_FREQ 32000 __BEGIN_DECLS diff --git a/boards/ark/fmu-v6x/src/init.c b/boards/ark/fmu-v6x/src/init.c index 0bd1295978..fddc066d1e 100644 --- a/boards/ark/fmu-v6x/src/init.c +++ b/boards/ark/fmu-v6x/src/init.c @@ -46,6 +46,7 @@ ****************************************************************************/ #include "board_config.h" +#include "spix_sync.h" #include #include @@ -279,5 +280,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif /* CONFIG_MMCSD */ + /* Configure the SPIX_SYNC output */ + spix_sync_servo_init(BOARD_SPIX_SYNC_FREQ); + spix_sync_servo_set(0, 150); + return OK; } diff --git a/boards/ark/fmu-v6x/src/spix_sync.c b/boards/ark/fmu-v6x/src/spix_sync.c new file mode 100644 index 0000000000..2bda386962 --- /dev/null +++ b/boards/ark/fmu-v6x/src/spix_sync.c @@ -0,0 +1,311 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Airmind nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file spix_sync.c +* +* +*/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include + +#include "spix_sync.h" + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(spix_sync_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) + +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 1000000 +#endif + +unsigned +spix_sync_timer_get_period(unsigned timer) +{ + return (rARR(timer)); +} + +static void spix_sync_timer_init_timer(unsigned timer, unsigned rate) +{ + if (spix_sync_timers[timer].base) { + + irqstate_t flags = px4_enter_critical_section(); + + /* enable the timer clock before we try to talk to it */ + + modifyreg32(spix_sync_timers[timer].clock_register, 0, spix_sync_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCR1(timer) = 0; + rCCR2(timer) = 0; + rCCR3(timer) = 0; + rCCR4(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((spix_sync_timers[timer].base == STM32_TIM1_BASE) || (spix_sync_timers[timer].base == STM32_TIM8_BASE)) { + + /* master output enable = on */ + + rBDTR(timer) = ATIM_BDTR_MOE; + } + + /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN + * then configure the timer to free-run at 1MHz. + * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. + */ + + rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; + + /* configure the timer to update at the desired rate */ + + rARR(timer) = (BOARD_PWM_FREQ / rate) - 1; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + px4_leave_critical_section(flags); + } + +} + +void spix_sync_channel_init(unsigned channel) +{ + /* Only initialize used channels */ + + if (spix_sync_channels[channel].timer_channel) { + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* configure the GPIO first */ + px4_arch_configgpio(spix_sync_channels[channel].gpio_out); + + uint16_t polarity = spix_sync_channels[channel].masks; + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + rCCER(timer) |= polarity | GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCER(timer) |= polarity | GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCER(timer) |= polarity | GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCER(timer) |= polarity | GTIM_CCER_CC4E; + break; + } + } +} + +int +spix_sync_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= arraySize(spix_sync_channels)) { + return -1; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].gpio_out == 0)) { + return -1; + } + + unsigned period = spix_sync_timer_get_period(timer); + + unsigned value = (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} + +unsigned spix_sync_servo_get(unsigned channel) +{ + if (channel >= 3) { + return 0; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + uint16_t value = 0; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].timer_channel == 0)) { + return 0; + } + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + unsigned period = spix_sync_timer_get_period(timer); + return ((value + 1) * 255 / period); +} + +int spix_sync_servo_init(unsigned rate) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + spix_sync_timer_init_timer(i, rate); + } + + /* now init channels */ + for (unsigned i = 0; i < arraySize(spix_sync_channels); i++) { + spix_sync_channel_init(i); + } + + spix_sync_servo_arm(true); + return OK; +} + +void +spix_sync_servo_deinit(void) +{ + /* disable the timers */ + spix_sync_servo_arm(false); +} +void +spix_sync_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + if (spix_sync_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + rCR1(i) = 0; + } + } + } +} diff --git a/boards/ark/fmu-v6x/src/spix_sync.h b/boards/ark/fmu-v6x/src/spix_sync.h new file mode 100644 index 0000000000..2e37c89086 --- /dev/null +++ b/boards/ark/fmu-v6x/src/spix_sync.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void spix_sync_channel_init(unsigned channel); +int spix_sync_servo_set(unsigned channel, uint8_t value); +unsigned spix_sync_servo_get(unsigned channel); +int spix_sync_servo_init(unsigned rate); +void spix_sync_servo_deinit(void); +void spix_sync_servo_arm(bool armed); +unsigned spix_sync_timer_get_period(unsigned timer); +__END_DECLS diff --git a/boards/ark/fmu-v6x/src/timer_config.cpp b/boards/ark/fmu-v6x/src/timer_config.cpp index 928f4916f0..3504c3569d 100644 --- a/boards/ark/fmu-v6x/src/timer_config.cpp +++ b/boards/ark/fmu-v6x/src/timer_config.cpp @@ -60,8 +60,8 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer5, DMA{DMA::Index1}), initIOTimer(Timer::Timer4, DMA{DMA::Index1}), initIOTimer(Timer::Timer12), - initIOTimer(Timer::Timer1), - initIOTimer(Timer::Timer2), + //initIOTimer(Timer::Timer1), + //initIOTimer(Timer::Timer2), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { @@ -73,8 +73,18 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), - initIOTimerChannelCapture(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers, timer_io_channels); + + +constexpr io_timers_t spix_sync_timers[MAX_SPIX_SYNC_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +constexpr timer_io_channels_t spix_sync_channels[MAX_SPIX_SYNC_TIMERS] = { + initIOTimerChannel(spix_sync_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), +}; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 90b354d882..9ea7a04228 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,13 +50,13 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 3150, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI0{"wq:SPI0", 2352, -1}; -static constexpr wq_config_t SPI1{"wq:SPI1", 2352, -2}; -static constexpr wq_config_t SPI2{"wq:SPI2", 2352, -3}; -static constexpr wq_config_t SPI3{"wq:SPI3", 2352, -4}; -static constexpr wq_config_t SPI4{"wq:SPI4", 2352, -5}; -static constexpr wq_config_t SPI5{"wq:SPI5", 2352, -6}; -static constexpr wq_config_t SPI6{"wq:SPI6", 2352, -7}; +static constexpr wq_config_t SPI0{"wq:SPI0", 2392, -1}; +static constexpr wq_config_t SPI1{"wq:SPI1", 2392, -2}; +static constexpr wq_config_t SPI2{"wq:SPI2", 2392, -3}; +static constexpr wq_config_t SPI3{"wq:SPI3", 2392, -4}; +static constexpr wq_config_t SPI4{"wq:SPI4", 2392, -5}; +static constexpr wq_config_t SPI5{"wq:SPI5", 2392, -6}; +static constexpr wq_config_t SPI6{"wq:SPI6", 2392, -7}; static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8}; static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9}; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 3f5d6676b7..31d6e9b1c7 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -58,6 +58,9 @@ __BEGIN_DECLS #define MAX_LED_TIMERS 2 #define MAX_TIMER_LED_CHANNELS 6 +#define MAX_SPIX_SYNC_TIMERS 2 +#define MAX_TIMER_SPIX_SYNC_CHANNELS 2 + #define IO_TIMER_ALL_MODES_CHANNELS 0 /* TIM_DMA_Base_address TIM DMA Base Address */ @@ -134,6 +137,10 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; +__EXPORT extern const io_timers_t spix_sync_timers[MAX_SPIX_SYNC_TIMERS]; +__EXPORT extern const io_timers_channel_mapping_t spix_sync_channel_mapping; +__EXPORT extern const timer_io_channels_t spix_sync_channels[MAX_TIMER_SPIX_SYNC_CHANNELS]; + __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context); diff --git a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt index 8ffd2f7328..113ebc6d65 100644 --- a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2023 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 @@ -35,6 +35,7 @@ px4_add_module( MAIN icm42688p COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD SRCS icm42688p_main.cpp ICM42688P.cpp diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index a414f28e9d..fa23ca4afd 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -40,6 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } +static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) : SPI(config), I2CSPIDriver(config), @@ -51,6 +56,15 @@ ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) : _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } + if (config.custom1 != 0) { + _enable_clock_input = true; + _input_clock_freq = config.custom1; + ConfigureCLKIN(); + + } else { + _enable_clock_input = false; + } + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); } @@ -96,6 +110,7 @@ void ICM42688P::print_status() I2CSPIDriverBase::print_status(); PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled"); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); @@ -334,6 +349,22 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples) } } +void ICM42688P::ConfigureCLKIN() +{ + for (auto &r0 : _register_bank0_cfg) { + if (r0.reg == Register::BANK_0::INTF_CONFIG1) { + r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE; + } + } + + for (auto &r1 : _register_bank1_cfg) { + if (r1.reg == Register::BANK_1::INTF_CONFIG5) { + r1.set_bits = PIN9_FUNCTION_BIT::CLKIN; + r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9; + } + } +} + void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { if (bank != _last_register_bank || force) { @@ -544,6 +575,10 @@ bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // Packet does not contain a new and valid extended 20-bit data valid = false; + } else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) { + // Packet does not contain ODR timestamp + valid = false; + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { // accel ODR changed valid = false; @@ -606,13 +641,22 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT; // 18-bits of accelerometer data bool scale_20bit = false; // first pass for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + // 20 bit hires mode // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) // Accel data is 18 bit () @@ -693,13 +737,22 @@ void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = 0; - gyro.dt = FIFO_SAMPLE_DT; // 20-bits of gyroscope data bool scale_20bit = false; // first pass for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + // 20 bit hires mode // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F); diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index b3b21e2db2..0e10b17d6d 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -74,6 +74,8 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; + static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; @@ -113,6 +115,7 @@ private: bool Configure(); void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); + void ConfigureCLKIN(); void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); } @@ -156,6 +159,9 @@ private: hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; + bool _enable_clock_input{false}; + float _input_clock_freq{0.f}; + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; px4::atomic _drdy_timestamp_sample{0}; @@ -173,18 +179,20 @@ private: int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{14}; + static constexpr uint8_t size_register_bank0_cfg{16}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR }, { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD }, { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW }, { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD }, - { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 }, + { Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN }, { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime { Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, @@ -193,10 +201,11 @@ private: }; uint8_t _checked_register_bank1{0}; - static constexpr uint8_t size_register_bank1_cfg{1}; + static constexpr uint8_t size_register_bank1_cfg{2}; register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { // Register | Set bits, Clear bits { Register::BANK_1::GYRO_CONFIG_STATIC2, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 }, + { Register::BANK_1::INTF_CONFIG5, 0, 0 }, }; uint8_t _checked_register_bank2{0}; diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 47e4499547..3874d6e470 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -90,6 +90,7 @@ enum class BANK_0 : uint8_t { GYRO_CONFIG1 = 0x51, GYRO_ACCEL_CONFIG0 = 0x52, ACCEL_CONFIG1 = 0x53, + TMST_CONFIG = 0x54, FIFO_CONFIG1 = 0x5F, FIFO_CONFIG2 = 0x60, @@ -108,7 +109,6 @@ enum class BANK_0 : uint8_t { enum class BANK_1 : uint8_t { GYRO_CONFIG_STATIC2 = 0x0B, - INTF_CONFIG5 = 0x7B, }; @@ -152,6 +152,12 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +enum INTF_CONFIG1_BIT : uint8_t { + RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required + CLKSEL = Bit0, + CLKSEL_CLEAR = Bit1, +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode @@ -226,11 +232,21 @@ enum ACCEL_CONFIG1_BIT : uint8_t { ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order }; +// TMST_CONFIG +enum TMST_CONFIG_BIT : uint8_t { + TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value + TMST_RES = Bit3, // 0: 1us resolution, 1: 16us resolution or 1 RTC period when clock is used + TMST_DELTA_EN = Bit2, // 1: Time Stamp delta enable + TMST_FSYNC_EN = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled + TMST_EN = Bit0, // 1: Time Stamp register enable (default) +}; + // FIFO_CONFIG1 enum FIFO_CONFIG1_BIT : uint8_t { FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_WM_GT_TH = Bit5, FIFO_HIRES_EN = Bit4, + FIFO_TMST_FSYNC_EN = Bit3, FIFO_TEMP_EN = Bit2, FIFO_GYRO_EN = Bit1, FIFO_ACCEL_EN = Bit0, @@ -277,6 +293,11 @@ enum GYRO_CONFIG_STATIC2_BIT : uint8_t { GYRO_NF_DIS = Bit0, }; +// PIN9_FUNCTION +enum PIN9_FUNCTION_BIT : uint8_t { + FSYNC_PIN9 = Bit1, + CLKIN = Bit2, +}; //---------------- BANK2 Register bits @@ -321,7 +342,7 @@ enum FIFO_HEADER_BIT : uint8_t { HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel - HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet }; diff --git a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp index 615c408305..7a915b8325 100644 --- a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp +++ b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -43,6 +43,7 @@ void ICM42688P::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -53,8 +54,12 @@ extern "C" int icm42688p_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) { switch (ch) { + case 'C': + cli.custom1 = atoi(cli.optArg()); + break; + case 'R': cli.rotation = (enum Rotation)atoi(cli.optArg()); break; diff --git a/src/drivers/imu/invensense/iim42652/CMakeLists.txt b/src/drivers/imu/invensense/iim42652/CMakeLists.txt index 1ce153c6f6..a9e99a9efd 100644 --- a/src/drivers/imu/invensense/iim42652/CMakeLists.txt +++ b/src/drivers/imu/invensense/iim42652/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN iim42652 COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD SRCS iim42652_main.cpp IIM42652.cpp diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.cpp b/src/drivers/imu/invensense/iim42652/IIM42652.cpp index dabd688715..d7a2172a88 100644 --- a/src/drivers/imu/invensense/iim42652/IIM42652.cpp +++ b/src/drivers/imu/invensense/iim42652/IIM42652.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 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 @@ -40,6 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } +static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + IIM42652::IIM42652(const I2CSPIDriverConfig &config) : SPI(config), I2CSPIDriver(config), @@ -51,6 +56,15 @@ IIM42652::IIM42652(const I2CSPIDriverConfig &config) : _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } + if (config.custom1 != 0) { + _enable_clock_input = true; + _input_clock_freq = config.custom1; + ConfigureCLKIN(); + + } else { + _enable_clock_input = false; + } + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); } @@ -96,6 +110,7 @@ void IIM42652::print_status() I2CSPIDriverBase::print_status(); PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled"); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); @@ -122,7 +137,7 @@ int IIM42652::probe() if (bank >= 1 && bank <= 3) { DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); // force bank selection and retry - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true); } } } @@ -171,21 +186,9 @@ void IIM42652::RunImpl() case STATE::CONFIGURE: if (Configure()) { - // if configure succeeded then start reading from FIFO - _state = STATE::FIFO_READ; - - if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; - - // backup schedule as a watchdog timeout - ScheduleDelayed(100_ms); - - } else { - _data_ready_interrupt_enabled = false; - ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); - } - - FIFOReset(); + // if configure succeeded then reset the FIFO + _state = STATE::FIFO_RESET; + ScheduleDelayed(1_ms); } else { // CONFIGURE not complete @@ -202,6 +205,24 @@ void IIM42652::RunImpl() break; + case STATE::FIFO_RESET: + + _state = STATE::FIFO_READ; + FIFOReset(); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + break; + case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; uint8_t samples = 0; @@ -328,6 +349,22 @@ void IIM42652::ConfigureFIFOWatermark(uint8_t samples) } } +void IIM42652::ConfigureCLKIN() +{ + for (auto &r0 : _register_bank0_cfg) { + if (r0.reg == Register::BANK_0::INTF_CONFIG1) { + r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE; + } + } + + for (auto &r1 : _register_bank1_cfg) { + if (r1.reg == Register::BANK_1::INTF_CONFIG5) { + r1.set_bits = PIN9_FUNCTION_BIT::CLKIN; + r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9; + } + } +} + void IIM42652::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { if (bank != _last_register_bank || force) { @@ -471,7 +508,7 @@ uint16_t IIM42652::FIFOReadCount() // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -485,7 +522,7 @@ bool IIM42652::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -538,6 +575,10 @@ bool IIM42652::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // Packet does not contain a new and valid extended 20-bit data valid = false; + } else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) { + // Packet does not contain ODR timestamp + valid = false; + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { // accel ODR changed valid = false; @@ -600,13 +641,22 @@ void IIM42652::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT; // 18-bits of accelerometer data bool scale_20bit = false; // first pass for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + // 20 bit hires mode // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) // Accel data is 18 bit () @@ -687,13 +737,22 @@ void IIM42652::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = 0; - gyro.dt = FIFO_SAMPLE_DT; // 20-bits of gyroscope data bool scale_20bit = false; // first pass for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + // 20 bit hires mode // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F); diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.hpp b/src/drivers/imu/invensense/iim42652/IIM42652.hpp index 9ba671d093..fc7927a34c 100644 --- a/src/drivers/imu/invensense/iim42652/IIM42652.hpp +++ b/src/drivers/imu/invensense/iim42652/IIM42652.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 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 @@ -74,6 +74,8 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; + static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; @@ -113,11 +115,12 @@ private: bool Configure(); void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); + void ConfigureCLKIN(); void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); - void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } - void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); } - void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); } + void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); } + void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); } static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); @@ -156,7 +159,10 @@ private: hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; + bool _enable_clock_input{false}; + float _input_clock_freq{0.f}; + + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; @@ -165,6 +171,7 @@ private: RESET, WAIT_FOR_RESET, CONFIGURE, + FIFO_RESET, FIFO_READ, } _state{STATE::RESET}; @@ -172,29 +179,33 @@ private: int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{13}; + static constexpr uint8_t size_register_bank0_cfg{16}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR }, { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD }, { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW }, { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD }, - { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 }, + { Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN }, { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime { Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, + { Register::BANK_0::INT_CONFIG1, 0, INT_CONFIG1_BIT::INT_ASYNC_RESET }, { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, }; uint8_t _checked_register_bank1{0}; - static constexpr uint8_t size_register_bank1_cfg{1}; + static constexpr uint8_t size_register_bank1_cfg{2}; register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { // Register | Set bits, Clear bits { Register::BANK_1::GYRO_CONFIG_STATIC2, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 }, + { Register::BANK_1::INTF_CONFIG5, 0, 0 }, }; uint8_t _checked_register_bank2{0}; diff --git a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp index 7f59099e2f..79bd024101 100644 --- a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp +++ b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp @@ -90,12 +90,14 @@ enum class BANK_0 : uint8_t { GYRO_CONFIG1 = 0x51, GYRO_ACCEL_CONFIG0 = 0x52, ACCEL_CONFIG1 = 0x53, + TMST_CONFIG = 0x54, FIFO_CONFIG1 = 0x5F, FIFO_CONFIG2 = 0x60, FIFO_CONFIG3 = 0x61, INT_CONFIG0 = 0x63, + INT_CONFIG1 = 0x64, INT_SOURCE0 = 0x65, @@ -107,7 +109,6 @@ enum class BANK_0 : uint8_t { enum class BANK_1 : uint8_t { GYRO_CONFIG_STATIC2 = 0x0B, - INTF_CONFIG5 = 0x7B, }; @@ -151,6 +152,12 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +enum INTF_CONFIG1_BIT : uint8_t { + RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required + CLKSEL = Bit0, + CLKSEL_CLEAR = Bit1, +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode @@ -225,11 +232,21 @@ enum ACCEL_CONFIG1_BIT : uint8_t { ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order }; +// TMST_CONFIG +enum TMST_CONFIG_BIT : uint8_t { + TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value + TMST_RES = Bit3, // 0: 1us resolution, 1: 16us resolution + TMST_DELTA_EN = Bit2, // 1: Time Stamp delta enable + TMST_FSYNC_EN = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled + TMST_EN = Bit0, // 1: Time Stamp register enable (default) +}; + // FIFO_CONFIG1 enum FIFO_CONFIG1_BIT : uint8_t { FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_WM_GT_TH = Bit5, FIFO_HIRES_EN = Bit4, + FIFO_TMST_FSYNC_EN = Bit3, FIFO_TEMP_EN = Bit2, FIFO_GYRO_EN = Bit1, FIFO_ACCEL_EN = Bit0, @@ -241,6 +258,11 @@ enum INT_CONFIG0_BIT : uint8_t { CLEAR_ON_FIFO_READ = Bit3, }; +// INT_CONFIG1 +enum INT_CONFIG1_BIT : uint8_t { + INT_ASYNC_RESET = Bit4, +}; + // INT_SOURCE0 enum INT_SOURCE0_BIT : uint8_t { UI_FSYNC_INT1_EN = Bit6, @@ -254,10 +276,12 @@ enum INT_SOURCE0_BIT : uint8_t { // REG_BANK_SEL enum REG_BANK_SEL_BIT : uint8_t { - USER_BANK_0 = 0, // 0: Select USER BANK 0. - USER_BANK_1 = Bit4, // 1: Select USER BANK 1. - USER_BANK_2 = Bit5, // 2: Select USER BANK 2. - USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3. + // 2:0 BANK_SEL + BANK_SEL_0 = 0, // 000: Bank 0 (default) + BANK_SEL_1 = Bit0, // 001: Bank 1 + BANK_SEL_2 = Bit1, // 010: Bank 2 + BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3 + BANK_SEL_4 = Bit2, // 100: Bank 4 }; @@ -269,6 +293,11 @@ enum GYRO_CONFIG_STATIC2_BIT : uint8_t { GYRO_NF_DIS = Bit0, }; +// PIN9_FUNCTION +enum PIN9_FUNCTION_BIT : uint8_t { + FSYNC_PIN9 = Bit1, + CLKIN = Bit2, +}; //---------------- BANK2 Register bits @@ -313,7 +342,7 @@ enum FIFO_HEADER_BIT : uint8_t { HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel - HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet }; diff --git a/src/drivers/imu/invensense/iim42652/iim42652_main.cpp b/src/drivers/imu/invensense/iim42652/iim42652_main.cpp index ae88e7e709..9521f8d8fe 100644 --- a/src/drivers/imu/invensense/iim42652/iim42652_main.cpp +++ b/src/drivers/imu/invensense/iim42652/iim42652_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 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 @@ -43,6 +43,7 @@ void IIM42652::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -53,8 +54,12 @@ extern "C" int iim42652_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) { switch (ch) { + case 'C': + cli.custom1 = atoi(cli.optArg()); + break; + case 'R': cli.rotation = (enum Rotation)atoi(cli.optArg()); break;