@ -45,8 +45,8 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro
@@ -45,8 +45,8 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro
SPI ( MODULE_NAME , nullptr , bus , device , spi_mode , bus_frequency ) ,
I2CSPIDriver ( MODULE_NAME , px4 : : device_bus_to_wq ( get_device_id ( ) ) , bus_option , bus ) ,
_drdy_gpio ( drdy_gpio ) ,
_px4_accel ( get_device_id ( ) , ORB_PRIO_VERY_ HIGH , rotation ) ,
_px4_gyro ( get_device_id ( ) , ORB_PRIO_VERY_ HIGH , rotation )
_px4_accel ( get_device_id ( ) , ORB_PRIO_HIGH , rotation ) ,
_px4_gyro ( get_device_id ( ) , ORB_PRIO_HIGH , rotation )
{
set_device_type ( DRV_IMU_DEVTYPE_ICM20689 ) ;
@ -131,7 +131,7 @@ void ICM20689::RunImpl()
@@ -131,7 +131,7 @@ void ICM20689::RunImpl()
RegisterWrite ( Register : : PWR_MGMT_1 , PWR_MGMT_1_BIT : : DEVICE_RESET ) ;
_reset_timestamp = hrt_absolute_time ( ) ;
_state = STATE : : WAIT_FOR_RESET ;
ScheduleDelayed ( 100 ) ;
ScheduleDelayed ( 1 _ms ) ;
break ;
case STATE : : WAIT_FOR_RESET :
@ -147,14 +147,14 @@ void ICM20689::RunImpl()
@@ -147,14 +147,14 @@ void ICM20689::RunImpl()
} else {
// RESET not complete
if ( hrt_elapsed_time ( & _reset_timestamp ) > 10 _ms ) {
PX4_ERR ( " Reset failed, retrying " ) ;
if ( hrt_elapsed_time ( & _reset_timestamp ) > 100 _ms ) {
PX4_DEBUG ( " Reset failed, retrying " ) ;
_state = STATE : : RESET ;
ScheduleDelayed ( 10 _ms ) ;
ScheduleDelayed ( 100 _ms ) ;
} else {
PX4_DEBUG ( " Reset not complete, check again in 1 ms " ) ;
ScheduleDelayed ( 1 _ms ) ;
PX4_DEBUG ( " Reset not complete, check again in 10 ms " ) ;
ScheduleDelayed ( 10 _ms ) ;
}
}
@ -180,8 +180,8 @@ void ICM20689::RunImpl()
@@ -180,8 +180,8 @@ void ICM20689::RunImpl()
} else {
PX4_DEBUG ( " Configure failed, retrying " ) ;
// try again in 1 ms
ScheduleDelayed ( 1 _ms ) ;
// try again in 10 ms
ScheduleDelayed ( 10 _ms ) ;
}
break ;
@ -195,7 +195,14 @@ void ICM20689::RunImpl()
@@ -195,7 +195,14 @@ void ICM20689::RunImpl()
ScheduleDelayed ( 10 _ms ) ;
// timestamp set in data ready interrupt
samples = _fifo_read_samples . load ( ) ;
if ( ! _force_fifo_count_check ) {
samples = _fifo_read_samples . load ( ) ;
} else {
const uint16_t fifo_count = FIFOReadCount ( ) ;
samples = ( fifo_count / sizeof ( FIFO : : DATA ) / SAMPLES_PER_TRANSFER ) * SAMPLES_PER_TRANSFER ; // round down to nearest
}
timestamp_sample = _fifo_watermark_interrupt_timestamp ;
}
@ -208,13 +215,7 @@ void ICM20689::RunImpl()
@@ -208,13 +215,7 @@ void ICM20689::RunImpl()
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
timestamp_sample = hrt_absolute_time ( ) ;
const uint16_t fifo_count = FIFOReadCount ( ) ;
if ( fifo_count = = 0 ) {
failure = true ;
perf_count ( _fifo_empty_perf ) ;
}
samples = ( fifo_count / sizeof ( FIFO : : DATA ) / 2 ) * 2 ; // round down to nearest 2
samples = ( fifo_count / sizeof ( FIFO : : DATA ) / SAMPLES_PER_TRANSFER ) * SAMPLES_PER_TRANSFER ; // round down to nearest
}
if ( samples > FIFO_MAX_SAMPLES ) {
@ -223,13 +224,17 @@ void ICM20689::RunImpl()
@@ -223,13 +224,17 @@ void ICM20689::RunImpl()
failure = true ;
FIFOReset ( ) ;
} else if ( samples > = 2 ) {
// require at least 2 samples (we want at least 1 new accel sample per transfer)
} else if ( samples > = SAMPLES_PER_TRANSFER ) {
// require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer)
if ( ! FIFORead ( timestamp_sample , samples ) ) {
failure = true ;
_px4_accel . increase_error_count ( ) ;
_px4_gyro . increase_error_count ( ) ;
}
} else if ( samples = = 0 ) {
failure = true ;
perf_count ( _fifo_empty_perf ) ;
}
if ( failure | | hrt_elapsed_time ( & _last_config_check_timestamp ) > 10 _ms ) {
@ -291,23 +296,23 @@ void ICM20689::ConfigureGyro()
@@ -291,23 +296,23 @@ void ICM20689::ConfigureGyro()
switch ( FS_SEL ) {
case FS_SEL_250_DPS :
_px4_gyro . set_scale ( math : : radians ( 1.0 f / 131.f ) ) ;
_px4_gyro . set_scale ( math : : radians ( 1.f / 131.f ) ) ;
_px4_gyro . set_range ( math : : radians ( 250.f ) ) ;
break ;
case FS_SEL_500_DPS :
_px4_gyro . set_scale ( math : : radians ( 1.0 f / 65.5f ) ) ;
_px4_gyro . set_scale ( math : : radians ( 1.f / 65.5f ) ) ;
_px4_gyro . set_range ( math : : radians ( 500.f ) ) ;
break ;
case FS_SEL_1000_DPS :
_px4_gyro . set_scale ( math : : radians ( 1.0 f / 32.8f ) ) ;
_px4_gyro . set_range ( math : : radians ( 1000.0 f ) ) ;
_px4_gyro . set_scale ( math : : radians ( 1.f / 32.8f ) ) ;
_px4_gyro . set_range ( math : : radians ( 1000.f ) ) ;
break ;
case FS_SEL_2000_DPS :
_px4_gyro . set_scale ( math : : radians ( 1.0 f / 16.4f ) ) ;
_px4_gyro . set_range ( math : : radians ( 2000.0 f ) ) ;
_px4_gyro . set_scale ( math : : radians ( 1.f / 16.4f ) ) ;
_px4_gyro . set_range ( math : : radians ( 2000.f ) ) ;
break ;
}
}
@ -318,16 +323,19 @@ void ICM20689::ConfigureSampleRate(int sample_rate)
@@ -318,16 +323,19 @@ void ICM20689::ConfigureSampleRate(int sample_rate)
sample_rate = 1000 ; // default to 1 kHz
}
_fifo_empty_interval_us = math : : max ( ( ( 1000000 / sample_rate ) / 250 ) * 250 , 250 ) ; // round down to nearest 250 us
_fifo_gyro_samples = math : : min ( _fifo_empty_interval_us / ( 1000000 / GYRO_RATE ) , FIFO_MAX_SAMPLES ) ;
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT ;
_fifo_empty_interval_us = math : : max ( roundf ( ( 1e6 f / ( float ) sample_rate ) / min_interval ) * min_interval , min_interval ) ;
_fifo_gyro_samples = math : : min ( ( float ) _fifo_empty_interval_us / ( 1e6 f / GYRO_RATE ) , ( float ) FIFO_MAX_SAMPLES ) ;
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * ( 1000000 / GYRO_RATE ) ;
_fifo_empty_interval_us = _fifo_gyro_samples * ( 1e6 f / GYRO_RATE ) ;
_fifo_accel_samples = math : : min ( _fifo_empty_interval_us / ( 1000000 / ACCEL_RATE ) , FIFO_MAX_SAMPLES ) ;
_fifo_accel_samples = math : : min ( _fifo_empty_interval_us / ( 1e6 f / ACCEL_RATE ) , ( float ) FIFO_MAX_SAMPLES ) ;
_px4_accel . set_update_rate ( 1000000 / _fifo_empty_interval_us ) ;
_px4_gyro . set_update_rate ( 1000000 / _fifo_empty_interval_us ) ;
_px4_accel . set_update_rate ( 1e6 f / _fifo_empty_interval_us ) ;
_px4_gyro . set_update_rate ( 1e6 f / _fifo_empty_interval_us ) ;
}
bool ICM20689 : : Configure ( )
@ -354,14 +362,14 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -354,14 +362,14 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20689 : : DataReady ( )
{
perf_count ( _drdy_interval_perf ) ;
if ( _data_ready_count . fetch_add ( 1 ) > = ( _fifo_gyro_samples - 1 ) ) {
_data_ready_count . store ( 0 ) ;
_fifo_watermark_interrupt_timestamp = hrt_absolute_time ( ) ;
_fifo_read_samples . store ( _fifo_gyro_samples ) ;
ScheduleNow ( ) ;
}
perf_count ( _drdy_interval_perf ) ;
}
bool ICM20689 : : DataReadyInterruptConfigure ( )
@ -370,8 +378,8 @@ bool ICM20689::DataReadyInterruptConfigure()
@@ -370,8 +378,8 @@ bool ICM20689::DataReadyInterruptConfigure()
return false ;
}
// Setup data ready on ris ing edge
return px4_arch_gpiosetevent ( _drdy_gpio , true , fals e, true , & ICM20689 : : DataReadyInterruptCallback , this ) = = 0 ;
// Setup data ready on fall ing edge
return px4_arch_gpiosetevent ( _drdy_gpio , false , tru e, true , & ICM20689 : : DataReadyInterruptCallback , this ) = = 0 ;
}
bool ICM20689 : : DataReadyInterruptDisable ( )
@ -389,12 +397,12 @@ bool ICM20689::RegisterCheck(const register_config_t ®_cfg, bool notify)
@@ -389,12 +397,12 @@ bool ICM20689::RegisterCheck(const register_config_t ®_cfg, bool notify)
const uint8_t reg_value = RegisterRead ( reg_cfg . reg ) ;
if ( reg_cfg . set_bits & & ! ( reg_value & reg_cfg . set_bits ) ) {
if ( reg_cfg . set_bits & & ( ( reg_value & reg_cfg . set_bits ) ! = reg_cfg . set_bits ) ) {
PX4_DEBUG ( " 0x%02hhX: 0x%02hhX (0x%02hhX not set) " , ( uint8_t ) reg_cfg . reg , reg_value , reg_cfg . set_bits ) ;
success = false ;
}
if ( reg_cfg . clear_bits & & ( reg_value & reg_cfg . clear_bits ) ) {
if ( reg_cfg . clear_bits & & ( ( reg_value & reg_cfg . clear_bits ) ! = 0 ) ) {
PX4_DEBUG ( " 0x%02hhX: 0x%02hhX (0x%02hhX not cleared) " , ( uint8_t ) reg_cfg . reg , reg_value , reg_cfg . clear_bits ) ;
success = false ;
}
@ -402,13 +410,6 @@ bool ICM20689::RegisterCheck(const register_config_t ®_cfg, bool notify)
@@ -402,13 +410,6 @@ bool ICM20689::RegisterCheck(const register_config_t ®_cfg, bool notify)
if ( ! success ) {
RegisterSetAndClearBits ( reg_cfg . reg , reg_cfg . set_bits , reg_cfg . clear_bits ) ;
if ( reg_cfg . reg = = Register : : ACCEL_CONFIG ) {
ConfigureAccel ( ) ;
} else if ( reg_cfg . reg = = Register : : GYRO_CONFIG ) {
ConfigureGyro ( ) ;
}
if ( notify ) {
perf_count ( _bad_register_perf ) ;
_px4_accel . increase_error_count ( ) ;
@ -449,16 +450,6 @@ void ICM20689::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cl
@@ -449,16 +450,6 @@ void ICM20689::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cl
RegisterWrite ( reg , val ) ;
}
void ICM20689 : : RegisterSetBits ( Register reg , uint8_t setbits )
{
RegisterSetAndClearBits ( reg , setbits , 0 ) ;
}
void ICM20689 : : RegisterClearBits ( Register reg , uint8_t clearbits )
{
RegisterSetAndClearBits ( reg , 0 , clearbits ) ;
}
uint16_t ICM20689 : : FIFOReadCount ( )
{
// read FIFO count
@ -477,7 +468,7 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
@@ -477,7 +468,7 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
{
perf_begin ( _transfer_perf ) ;
FIFOTransferBuffer buffer { } ;
const size_t transfer_size = math : : min ( samples * sizeof ( FIFO : : DATA ) + 1 , FIFO : : SIZE ) ;
const size_t transfer_size = math : : min ( samples * sizeof ( FIFO : : DATA ) + 3 , FIFO : : SIZE ) ;
if ( transfer ( ( uint8_t * ) & buffer , ( uint8_t * ) & buffer , transfer_size ) ! = PX4_OK ) {
perf_end ( _transfer_perf ) ;
@ -487,8 +478,41 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
@@ -487,8 +478,41 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
perf_end ( _transfer_perf ) ;
ProcessGyro ( timestamp_sample , buffer , samples ) ;
return ProcessAccel ( timestamp_sample , buffer , samples ) ;
const uint16_t fifo_count_bytes = combine ( buffer . FIFO_COUNTH , buffer . FIFO_COUNTL ) ;
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof ( FIFO : : DATA ) ;
if ( fifo_count_samples = = 0 ) {
perf_count ( _fifo_empty_perf ) ;
return false ;
}
if ( fifo_count_bytes > = FIFO : : SIZE ) {
perf_count ( _fifo_overflow_perf ) ;
FIFOReset ( ) ;
return false ;
}
const uint16_t valid_samples = math : : min ( samples , fifo_count_samples ) ;
if ( fifo_count_samples < samples ) {
// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
_force_fifo_count_check = true ;
} else if ( fifo_count_samples > samples + 4 ) {
// if we're more than a few samples behind force FIFO_COUNT check
_force_fifo_count_check = true ;
} else {
// skip earlier FIFO_COUNT and trust DRDY count if we're in sync
_force_fifo_count_check = false ;
}
if ( valid_samples > 0 ) {
ProcessGyro ( timestamp_sample , buffer , valid_samples ) ;
return ProcessAccel ( timestamp_sample , buffer , valid_samples ) ;
}
return false ;
}
void ICM20689 : : FIFOReset ( )
@ -498,9 +522,8 @@ void ICM20689::FIFOReset()
@@ -498,9 +522,8 @@ void ICM20689::FIFOReset()
// FIFO_EN: disable FIFO
RegisterWrite ( Register : : FIFO_EN , 0 ) ;
// USER_CTRL: disable FIFO and reset all signal paths
RegisterSetAndClearBits ( Register : : USER_CTRL , USER_CTRL_BIT : : FIFO_RST | USER_CTRL_BIT : : SIG_COND_RST ,
USER_CTRL_BIT : : FIFO_EN ) ;
// USER_CTRL: reset FIFO
RegisterSetAndClearBits ( Register : : USER_CTRL , USER_CTRL_BIT : : FIFO_RST , USER_CTRL_BIT : : FIFO_EN ) ;
// reset while FIFO is disabled
_data_ready_count . store ( 0 ) ;
@ -521,7 +544,8 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
@@ -521,7 +544,8 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
return ( memcmp ( & f0 . ACCEL_XOUT_H , & f1 . ACCEL_XOUT_H , 6 ) = = 0 ) ;
}
bool ICM20689 : : ProcessAccel ( const hrt_abstime & timestamp_sample , const FIFOTransferBuffer & buffer , uint8_t samples )
bool ICM20689 : : ProcessAccel ( const hrt_abstime & timestamp_sample , const FIFOTransferBuffer & buffer ,
const uint8_t samples )
{
PX4Accelerometer : : FIFOSample accel ;
accel . timestamp_sample = timestamp_sample ;
@ -532,8 +556,8 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans
@@ -532,8 +556,8 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans
// accel data is doubled in FIFO, but might be shifted
int accel_first_sample = 1 ;
if ( samples > = 3 ) {
if ( fifo_accel_equal ( buffer . f [ 0 ] , buffer . f [ 1 ] ) ) {
if ( samples > = 4 ) {
if ( fifo_accel_equal ( buffer . f [ 0 ] , buffer . f [ 1 ] ) & & fifo_accel_equal ( buffer . f [ 2 ] , buffer . f [ 3 ] ) ) {
// [A0, A1, A2, A3]
// A0==A1, A2==A3
accel_first_sample = 1 ;
@ -572,7 +596,7 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans
@@ -572,7 +596,7 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans
return ! bad_data ;
}
void ICM20689 : : ProcessGyro ( const hrt_abstime & timestamp_sample , const FIFOTransferBuffer & buffer , uint8_t samples )
void ICM20689 : : ProcessGyro ( const hrt_abstime & timestamp_sample , const FIFOTransferBuffer & buffer , const uint8_t samples )
{
PX4Gyroscope : : FIFOSample gyro ;
gyro . timestamp_sample = timestamp_sample ;