sensors: simplify timestamp data validator handling

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;

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 (time_now_us > _last_error_message + 3_s) {
-
-				if (now - _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

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

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);
+	_accel.voter.get_best(time_now_us, &accel_best_index);
-	_gyro.voter.get_best(hrt_absolute_time(), &gyro_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();
 			}
 		}