fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Enable ARKV6X SPIX SYNC, enable icm426889 and iim42652 CLKIN

Browse Source
This commit is contained in:
alexklimaj 2022-10-17 19:15:30 -06:00 committed by Daniel Agar
parent 967c37ac17
commit 7c2da8d1ef
19 changed files with 648 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
boards/ark/fmu-v6x/init/rc.board_sensors
View File

@ -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

7
boards/ark/fmu-v6x/src/CMakeLists.txt
View File

@ -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
)

11
boards/ark/fmu-v6x/src/board_config.h
View File

@ -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

5
boards/ark/fmu-v6x/src/init.c
View File

@ -46,6 +46,7 @@
****************************************************************************/
#include "board_config.h"
#include "spix_sync.h"
#include <stdbool.h>
#include <stdio.h>
@ -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;
}

311
boards/ark/fmu-v6x/src/spix_sync.c Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <board_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/px4_macros.h>
#include <px4_arch/io_timer.h>
#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;
}
}
}
}

42
boards/ark/fmu-v6x/src/spix_sync.h Normal file
View File

@ -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

16
boards/ark/fmu-v6x/src/timer_config.cpp
View File

@ -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}),
};

14
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -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};

7
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
View File

@ -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);

3
src/drivers/imu/invensense/icm42688p/CMakeLists.txt
View File

@ -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

59
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -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 &timestamp_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 &timestamp_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 &timestamp_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);

17
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
View File

@ -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<hrt_abstime> _drdy_timestamp_sample{0};
@ -173,18 +179,20 @@ private:
int32_t _fifo_gyro_samples{static_cast<int32_t>(_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};

25
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
View File

@ -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
};

9
src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
View File

@ -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;

1
src/drivers/imu/invensense/iim42652/CMakeLists.txt
View File

@ -35,6 +35,7 @@ px4_add_module(
MAIN iim42652
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
#-DDEBUG_BUILD
SRCS
iim42652_main.cpp
IIM42652.cpp

101
src/drivers/imu/invensense/iim42652/IIM42652.cpp
View File

@ -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<uint8_t>(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 &timestamp_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 &timestamp_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 &timestamp_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 &timestamp_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);

27
src/drivers/imu/invensense/iim42652/IIM42652.hpp
View File

@ -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<hrt_abstime> _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<int32_t>(_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};

41
src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp
View File

@ -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
};

9
src/drivers/imu/invensense/iim42652/iim42652_main.cpp
View File

@ -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;