From a6696d339d05b58123e3125c0e1dec3c9f980588 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Dec 2016 13:42:37 +0100 Subject: [PATCH] Sensors app: Fix consistency checks. The sensors app assumed that all topics are published on boot which is not necessarily true and it assumed that all publications had valid data. This change ensures that topics are initialized as they update the first time and that the consistency check only runs on topics which carry valid data. --- src/modules/sensors/voted_sensors_update.cpp | 75 +++++++++++++------- src/modules/sensors/voted_sensors_update.h | 1 + 2 files changed, 49 insertions(+), 27 deletions(-) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index e84b3f1ff2..38be7f8f3f 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -401,6 +401,13 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + // First publication with data + if (_accel.priority[i] == 0) { + int32_t priority = 0; + orb_priority(_accel.subscription[i], &priority); + _accel.priority[i] = (uint8_t)priority; + } + if (accel_report.integral_dt != 0) { math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); vect_int = _board_rotation * vect_int; @@ -461,6 +468,13 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + // First publication with data + if (_gyro.priority[i] == 0) { + int32_t priority = 0; + orb_priority(_gyro.subscription[i], &priority); + _gyro.priority[i] = (uint8_t)priority; + } + if (gyro_report.integral_dt != 0) { math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); vect_int = _board_rotation * vect_int; @@ -522,6 +536,13 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + // First publication with data + if (_mag.priority[i] == 0) { + int32_t priority = 0; + orb_priority(_mag.subscription[i], &priority); + _mag.priority[i] = (uint8_t)priority; + } + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); vect = _mag_rotation[i] * vect; @@ -563,6 +584,13 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + // First publication with data + if (_baro.priority[i] == 0) { + int32_t priority = 0; + orb_priority(_baro.subscription[i], &priority); + _baro.priority[i] = (uint8_t)priority; + } + got_update = true; math::Vector<3> vect(baro_report.altitude, 0.f, 0.f); @@ -664,10 +692,6 @@ void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, Sens } } } - - int32_t priority; - orb_priority(sensor_data.subscription[i], &priority); - sensor_data.priority[i] = (uint8_t)priority; } sensor_data.subscription_count = group_count; @@ -761,21 +785,14 @@ void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) void VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt) { - // skip check if less than 2 sensors - if (_accel.subscription_count < 2) { - preflt.accel_inconsistency_m_s_s = 0.0f; - return; - - } - float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared - uint8_t check_index = 0; // the number of sensors the primary has been checked against + unsigned check_index = 0; // the number of sensors the primary has been checked against // Check each sensor against the primary for (unsigned sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if (sensor_index != _accel.last_best_vote) { + if ((_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) { float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary @@ -805,28 +822,26 @@ VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt) } } - // get the vector length of the largest difference and write to the combined sensor struct - preflt.accel_inconsistency_m_s_s = sqrtf(accel_diff_sum_max_sq); + // skip check if less than 2 sensors + if (check_index < 1) { + preflt.accel_inconsistency_m_s_s = 0.0f; + + } else { + // get the vector length of the largest difference and write to the combined sensor struct + preflt.accel_inconsistency_m_s_s = sqrtf(accel_diff_sum_max_sq); + } } void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) { - // skip check if less than 2 sensors - if (_gyro.subscription_count < 2) { - preflt.gyro_inconsistency_rad_s = 0.0f; - return; - - } - float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared - uint8_t check_index = 0; // the number of sensors the primary has been checked against - + unsigned check_index = 0; // the number of sensors the primary has been checked against // Check each sensor against the primary for (unsigned sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if (sensor_index != _gyro.last_best_vote) { + if ((_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) { float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary @@ -856,8 +871,14 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) } } - // get the vector length of the largest difference and write to the combined sensor struct - preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq); + // skip check if less than 2 sensors + if (check_index < 1) { + preflt.gyro_inconsistency_rad_s = 0.0f; + + } else { + // get the vector length of the largest difference and write to the combined sensor struct + preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq); + } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 80e0f462a1..d5797112ef 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -152,6 +152,7 @@ private: { for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { subscription[i] = -1; + priority[i] = 0; } }