fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 12:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: inconsistency checks explicitly skip disabled

Browse Source
This commit is contained in:
Daniel Agar
2020-05-06 09:31:09 -04:00
parent 5bd0df422d
commit cb180427cd
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+3 -3
View File
@@ -827,7 +827,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
for (int 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 ((_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
if (_accel.enabled[sensor_index] && (_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
@@ -876,7 +876,7 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
for (int 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 ((_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
if (_gyro.enabled[sensor_index] && (_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
@@ -925,7 +925,7 @@ void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
// Check each sensor against the primary
for (int i = 0; i < _mag.subscription_count; i++) {
// check that the sensor we are checking against is not the same as the primary
if ((_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
// calculate angle to 3D magnetic field vector of the primary sensor
Vector3f current_mag(_last_magnetometer[i].magnetometer_ga);
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();