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

drivers/imu: minimize unnecessary hrt calls

Browse Source
This commit is contained in:
Daniel Agar
2021-06-29 10:55:04 -04:00
parent 4cf8eb8226
commit 26d971b93a
27 changed files with 97 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
+4 -4
View File
@@ -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;
src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
+3 -3
View File
@@ -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;
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
+3 -3
View File
@@ -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;
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
+3 -3
View File
@@ -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;
src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
+2 -2
View File
@@ -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;
src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
+2 -2
View File
@@ -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;
src/drivers/imu/fxas21002c/FXAS21002C.cpp
+1 -1
View File
@@ -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
src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
+4 -2
View File
@@ -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
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+3 -3
View File
@@ -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;
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm20649/ICM20649.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm20689/ICM20689.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm20948/ICM20948.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
+5 -3
View File
@@ -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);
src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm42605/ICM42605.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+2 -2
View File
@@ -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;
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp
+5 -3
View File
@@ -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);
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
+4 -4
View File
@@ -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;
}
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
+4 -4
View File
@@ -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;
}