fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: simplify timestamp data validator handling

Browse Source
This commit is contained in:
Daniel Agar
2022-01-18 14:57:18 -05:00
parent 8067207ea6
commit b1d2a0cc4e
4 changed files with 21 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
+7 -7
View File
@@ -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