forked from Archive/PX4-Autopilot
Compare commits
1 Commits
main
...
pr-sensors
Author | SHA1 | Date |
---|---|---|
Daniel Agar | 26d971b93a |
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue