diff --git a/src/modules/sensors/data_validator/DataValidator.cpp b/src/modules/sensors/data_validator/DataValidator.cpp index 81871a1ae8..68ca806923 100644 --- a/src/modules/sensors/data_validator/DataValidator.cpp +++ b/src/modules/sensors/data_validator/DataValidator.cpp @@ -105,7 +105,7 @@ float DataValidator::confidence(uint64_t timestamp) _error_mask |= ERROR_FLAG_NO_DATA; ret = 0.0f; - } else if (timestamp - _time_last > _timeout_interval) { + } else if (timestamp > _time_last + _timeout_interval) { /* timed out - that's it */ _error_mask |= ERROR_FLAG_TIMEOUT; ret = 0.0f; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 83c1a75a10..8d4e605bd6 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -143,6 +143,8 @@ void VehicleAirData::Run() { perf_begin(_cycle_perf); + const hrt_abstime time_now_us = hrt_absolute_time(); + ParametersUpdate(); SensorCorrectionsUpdate(); @@ -155,7 +157,7 @@ void VehicleAirData::Run() if (!_advertised[uorb_index]) { // use data's timestamp to throttle advertisement checks - if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { + if ((_last_data[uorb_index].timestamp == 0) || (time_now_us > _last_data[uorb_index].timestamp + 1_s)) { if (_sensor_sub[uorb_index].advertised()) { if (uorb_index > 0) { /* the first always exists, but for each further sensor, add a new validator */ @@ -174,7 +176,7 @@ void VehicleAirData::Run() } } else { - _last_data[uorb_index].timestamp = hrt_absolute_time(); + _last_data[uorb_index].timestamp = time_now_us; } } } @@ -198,7 +200,7 @@ void VehicleAirData::Run() // check for the current best sensor int best_index = 0; - _voter.get_best(hrt_absolute_time(), &best_index); + _voter.get_best(time_now_us, &best_index); if (best_index >= 0) { if (_selected_sensor_sub_index != best_index) { @@ -286,9 +288,7 @@ void VehicleAirData::Run() if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { if (failover_index != -1) { - const hrt_abstime now = hrt_absolute_time(); - - if (now - _last_error_message > 3_s) { + if (time_now_us > _last_error_message + 3_s) { mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t", "BARO", failover_index, @@ -318,7 +318,7 @@ void VehicleAirData::Run() events::ID("sensor_failover_baro"), events::Log::Emergency, "Baro sensor #{1} failure: {2}", failover_index, failover_reason); - _last_error_message = now; + _last_error_message = time_now_us; } // reduce priority of failed sensor to the minimum diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 94d3f5ad5d..4f734e7af6 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -358,6 +358,8 @@ void VehicleMagnetometer::Run() { perf_begin(_cycle_perf); + const hrt_abstime time_now_us = hrt_absolute_time(); + ParametersUpdate(); // check vehicle status for changes to armed state @@ -381,12 +383,12 @@ void VehicleMagnetometer::Run() if (!_advertised[uorb_index]) { // use data's timestamp to throttle advertisement checks - if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { + if ((_last_data[uorb_index].timestamp == 0) || (time_now_us > _last_data[uorb_index].timestamp + 1_s)) { if (_sensor_sub[uorb_index].advertised()) { _advertised[uorb_index] = true; } else { - _last_data[uorb_index].timestamp = hrt_absolute_time(); + _last_data[uorb_index].timestamp = time_now_us; } } } @@ -445,7 +447,7 @@ void VehicleMagnetometer::Run() // check for the current best sensor int best_index = 0; - _voter.get_best(hrt_absolute_time(), &best_index); + _voter.get_best(time_now_us, &best_index); if (best_index >= 0) { if (_selected_sensor_sub_index != best_index) { @@ -494,9 +496,8 @@ void VehicleMagnetometer::Run() if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { if (failover_index != -1) { - const hrt_abstime now = hrt_absolute_time(); - if (now - _last_error_message > 3_s) { + if (time_now_us > _last_error_message + 3_s) { mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t", "MAG", failover_index, @@ -526,7 +527,7 @@ void VehicleMagnetometer::Run() events::ID("sensor_failover_mag"), events::Log::Emergency, "Magnetometer sensor #{1} failure: {2}", failover_index, failover_reason); - _last_error_message = now; + _last_error_message = time_now_us; } // reduce priority of failed sensor to the minimum diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index e9c8a7783e..d9e69608bc 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -140,6 +140,8 @@ void VotedSensorsUpdate::parametersUpdate() void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) { + const hrt_abstime time_now_us = hrt_absolute_time(); + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { vehicle_imu_s imu_report; @@ -176,10 +178,10 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; - _accel.voter.put(uorb_index, imu_report.timestamp_sample, _last_sensor_data[uorb_index].accelerometer_m_s2, + _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, imu_status.accel_error_count, _accel.priority[uorb_index]); - _gyro.voter.put(uorb_index, imu_report.timestamp_sample, _last_sensor_data[uorb_index].gyro_rad, + _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, imu_status.gyro_error_count, _gyro.priority[uorb_index]); } } @@ -187,8 +189,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) // find the best sensor int accel_best_index = -1; int gyro_best_index = -1; - _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); - _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); + _accel.voter.get_best(time_now_us, &accel_best_index); + _gyro.voter.get_best(time_now_us, &gyro_best_index); if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { // use sensor_selection to find best @@ -196,9 +198,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) // reset inconsistency checks against primary for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { _accel_diff[sensor_index].zero(); - } - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { _gyro_diff[sensor_index].zero(); } }