From 26d971b93ac441f6e2ef2a59d1b26acce2d5dd4e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jun 2021 10:55:04 -0400 Subject: [PATCH] drivers/imu: minimize unnecessary hrt calls --- src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp | 8 ++++---- src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp | 6 +++--- src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp | 8 ++++---- src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp | 6 +++--- src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp | 8 ++++---- src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp | 6 +++--- src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp | 4 ++-- src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp | 4 ++-- src/drivers/imu/fxas21002c/FXAS21002C.cpp | 2 +- src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp | 6 ++++-- src/drivers/imu/invensense/icm20602/ICM20602.cpp | 6 +++--- src/drivers/imu/invensense/icm20608g/ICM20608G.cpp | 8 ++++---- src/drivers/imu/invensense/icm20649/ICM20649.cpp | 8 ++++---- src/drivers/imu/invensense/icm20689/ICM20689.cpp | 8 ++++---- src/drivers/imu/invensense/icm20948/ICM20948.cpp | 8 ++++---- src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp | 8 +++++--- .../imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp | 8 ++++---- src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 8 ++++---- src/drivers/imu/invensense/icm42605/ICM42605.cpp | 8 ++++---- src/drivers/imu/invensense/icm42670p/ICM42670P.cpp | 8 ++++---- src/drivers/imu/invensense/icm42688p/ICM42688P.cpp | 4 ++-- src/drivers/imu/invensense/mpu6000/MPU6000.cpp | 8 ++++---- src/drivers/imu/invensense/mpu6500/MPU6500.cpp | 8 ++++---- src/drivers/imu/invensense/mpu9250/MPU9250.cpp | 8 ++++---- src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp | 8 +++++--- src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp | 8 ++++---- src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 8 ++++---- 27 files changed, 97 insertions(+), 91 deletions(-) diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index f5e92604c7..0001c7e060 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -208,7 +208,7 @@ void ADIS16448::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -232,7 +232,7 @@ void ADIS16448::RunImpl() if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) { // self test not finished, check again - if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) { + if ((now - _reset_timestamp) < 1000_ms) { ScheduleDelayed(45_ms); PX4_DEBUG("self test not complete, check again in 45 ms"); return; @@ -317,7 +317,7 @@ void ADIS16448::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -473,7 +473,7 @@ void ADIS16448::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 7df959803f..4f78da1404 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -148,7 +148,7 @@ void ADIS16470::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -201,7 +201,7 @@ void ADIS16470::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -341,7 +341,7 @@ void ADIS16470::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 77cf537302..6789c714e0 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -115,7 +115,7 @@ void BMI055_Accelerometer::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -148,7 +148,7 @@ void BMI055_Accelerometer::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -232,7 +232,7 @@ void BMI055_Accelerometer::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || (now - _last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -246,7 +246,7 @@ void BMI055_Accelerometer::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index d2ea514530..70953097fc 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -115,7 +115,7 @@ void BMI055_Gyroscope::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -148,7 +148,7 @@ void BMI055_Gyroscope::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -232,7 +232,7 @@ void BMI055_Gyroscope::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || (now - _last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8f0dc0068b..2d32802618 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -137,7 +137,7 @@ void BMI088_Accelerometer::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -170,7 +170,7 @@ void BMI088_Accelerometer::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -255,7 +255,7 @@ void BMI088_Accelerometer::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -269,7 +269,7 @@ void BMI088_Accelerometer::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 7dac23e578..ec081fa66b 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -116,7 +116,7 @@ void BMI088_Gyroscope::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -149,7 +149,7 @@ void BMI088_Gyroscope::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -233,7 +233,7 @@ void BMI088_Gyroscope::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp index 6d7e4d6739..f381b1d421 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp @@ -144,7 +144,7 @@ void BMI088_Accelerometer::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -177,7 +177,7 @@ void BMI088_Accelerometer::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index ad177c68e5..ded701cd7c 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -123,7 +123,7 @@ void BMI088_Gyroscope::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -156,7 +156,7 @@ void BMI088_Gyroscope::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 9f755f4d8d..6b0830f710 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -381,7 +381,7 @@ void FXAS21002C::RunImpl() _px4_gyro.set_error_count(perf_event_count(_bad_registers)); _px4_gyro.update(timestamp_sample, x_raw, y_raw, z_raw); - if (hrt_elapsed_time(&_last_temperature_update) > 100_ms) { + if ((timestamp_sample - _last_temperature_update) > 100_ms) { /* * The TEMP register contains an 8-bit 2's complement temperature value with a range * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index fc5a22d214..e4b5612d60 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -338,7 +338,7 @@ void FXOS8701CQ::RunImpl() _px4_accel.set_error_count(perf_event_count(_bad_registers)); _px4_accel.update(timestamp_sample, x, y, z); - if (hrt_elapsed_time(&_last_temperature_update) > 100_ms) { + if ((timestamp_sample - _last_temperature_update) > 100_ms) { /* * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor @@ -355,11 +355,13 @@ void FXOS8701CQ::RunImpl() #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - if (hrt_elapsed_time(&_mag_last_measure) >= 10_ms) { + if ((timestamp_sample - _mag_last_measure) >= 10_ms) { int16_t mag_x = swap16(raw_accel_mag_report.mx); int16_t mag_y = swap16(raw_accel_mag_report.my); int16_t mag_z = swap16(raw_accel_mag_report.mz); _px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z); + + _mag_last_measure = timestamp_sample; } #endif diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index c13ab0b800..6a426b1862 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -190,7 +190,7 @@ void ICM20602::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -223,7 +223,7 @@ void ICM20602::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -315,7 +315,7 @@ void ICM20602::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || (now - _last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 06fe4b048e..40da901c07 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -181,7 +181,7 @@ void ICM20608G::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -214,7 +214,7 @@ void ICM20608G::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -292,7 +292,7 @@ void ICM20608G::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -306,7 +306,7 @@ void ICM20608G::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index e38c9381b8..a28874cf02 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -147,7 +147,7 @@ void ICM20649::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -180,7 +180,7 @@ void ICM20649::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -267,7 +267,7 @@ void ICM20649::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) @@ -284,7 +284,7 @@ void ICM20649::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 8337ba2cc4..8e8fa1a14b 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -181,7 +181,7 @@ void ICM20689::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -214,7 +214,7 @@ void ICM20689::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -292,7 +292,7 @@ void ICM20689::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || (now - _last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -306,7 +306,7 @@ void ICM20689::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index d95cb636c3..0847cd9271 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -177,7 +177,7 @@ void ICM20948::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -216,7 +216,7 @@ void ICM20948::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -303,7 +303,7 @@ void ICM20948::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) @@ -322,7 +322,7 @@ void ICM20948::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 8abe78bfa7..5efe9bf40d 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -79,11 +79,13 @@ void ICM20948_AK09916::PrintInfo() void ICM20948_AK09916::Run() { + const hrt_abstime now = hrt_absolute_time(); + switch (_state) { case STATE::RESET: // CNTL3 SRST: Soft reset _icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL3, CNTL3_BIT::SRST); - _reset_timestamp = hrt_absolute_time(); + _reset_timestamp = now; _failure_count = 0; _state = STATE::READ_WHO_AM_I; ScheduleDelayed(100_ms); @@ -109,7 +111,7 @@ void ICM20948_AK09916::Run() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("AK09916 reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(1000_ms); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp index c5cd3555a0..b682aa1d1c 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp @@ -121,7 +121,7 @@ void ICM20948_I2C_Passthrough::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -141,7 +141,7 @@ void ICM20948_I2C_Passthrough::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -155,7 +155,7 @@ void ICM20948_I2C_Passthrough::RunImpl() break; case STATE::READ: { - if (hrt_elapsed_time(&_last_config_check_timestamp) > 1000_ms) { + if ((now - _last_config_check_timestamp) > 1000_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])) { _last_config_check_timestamp = now; @@ -169,7 +169,7 @@ void ICM20948_I2C_Passthrough::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 894c9cd7c2..14e1cd9e9e 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -142,7 +142,7 @@ void ICM40609D::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -175,7 +175,7 @@ void ICM40609D::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -260,7 +260,7 @@ void ICM40609D::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) ) { @@ -275,7 +275,7 @@ void ICM40609D::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 88ddb8b30e..e947678190 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -143,7 +143,7 @@ void ICM42605::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -176,7 +176,7 @@ void ICM42605::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -261,7 +261,7 @@ void ICM42605::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) ) { @@ -276,7 +276,7 @@ void ICM42605::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp index f66eb13e2e..73d9a9ee52 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp @@ -143,7 +143,7 @@ void ICM42670P::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -176,7 +176,7 @@ void ICM42670P::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -257,7 +257,7 @@ void ICM42670P::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || (now - _last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) ) { @@ -272,7 +272,7 @@ void ICM42670P::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 0c8e66f862..d1c25942a3 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -143,7 +143,7 @@ void ICM42688P::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -176,7 +176,7 @@ void ICM42688P::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index ffdb9bafb0..c5aad6700a 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -150,7 +150,7 @@ void MPU6000::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -183,7 +183,7 @@ void MPU6000::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -261,7 +261,7 @@ void MPU6000::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -275,7 +275,7 @@ void MPU6000::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 2f29db0f79..6c785ad85d 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -182,7 +182,7 @@ void MPU6500::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -215,7 +215,7 @@ void MPU6500::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -293,7 +293,7 @@ void MPU6500::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -307,7 +307,7 @@ void MPU6500::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 501f5f97db..f96fd91c21 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -212,7 +212,7 @@ void MPU9250::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -250,7 +250,7 @@ void MPU9250::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -328,7 +328,7 @@ void MPU9250::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -342,7 +342,7 @@ void MPU9250::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index 6428f77e09..270d3e58a4 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -79,11 +79,13 @@ void MPU9250_AK8963::PrintInfo() void MPU9250_AK8963::Run() { + const hrt_abstime now = hrt_absolute_time(); + switch (_state) { case STATE::RESET: // CNTL2 SRST: Soft reset _mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::SRST); - _reset_timestamp = hrt_absolute_time(); + _reset_timestamp = now; _failure_count = 0; _state = STATE::READ_WHO_AM_I; ScheduleDelayed(100_ms); @@ -123,7 +125,7 @@ void MPU9250_AK8963::Run() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("AK8963 reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(1000_ms); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp index 73a87a71db..1233ec6f6b 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -150,7 +150,7 @@ void MPU9250_I2C::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -183,7 +183,7 @@ void MPU9250_I2C::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -261,7 +261,7 @@ void MPU9250_I2C::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -275,7 +275,7 @@ void MPU9250_I2C::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; } diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index b5577eff63..fc7b437e9c 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -134,7 +134,7 @@ void LSM9DS1::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -156,7 +156,7 @@ void LSM9DS1::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + if ((now - _reset_timestamp) > 1000_ms) { PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; @@ -219,7 +219,7 @@ void LSM9DS1::RunImpl() } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (!success || ((now - _last_config_check_timestamp) > 100_ms)) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -233,7 +233,7 @@ void LSM9DS1::RunImpl() } else { // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + if ((now - _temperature_update_timestamp) >= 1_s) { UpdateTemperature(); _temperature_update_timestamp = now; }