fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-17 17:46:56 -05:00
parent 8067207ea6
commit b1d2a0cc4e
4 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/sensors/data_validator/DataValidator.cpp
View File

@ -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;

14
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
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

13
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -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

13
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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();
}
}