From 60a68d30c73362566c8b41f66c94967fb950d33b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 2 Oct 2017 10:30:28 +1100 Subject: [PATCH] sensors: Calculate and publish magnetometer inconsistency --- src/modules/sensors/sensors.cpp | 3 +- src/modules/sensors/voted_sensors_update.cpp | 48 ++++++++++++++++++++ src/modules/sensors/voted_sensors_update.h | 8 +++- 3 files changed, 57 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 066957e466..6216c27039 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -621,8 +621,8 @@ Sensors::run() /* advertise the sensor_preflight topic and make the initial publication */ preflt.accel_inconsistency_m_s_s = 0.0f; - preflt.gyro_inconsistency_rad_s = 0.0f; + preflt.mag_inconsistency_ga = 0.0f; _sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt); @@ -686,6 +686,7 @@ Sensors::run() if (!_armed) { _voted_sensors_update.calc_accel_inconsistency(preflt); _voted_sensors_update.calc_gyro_inconsistency(preflt); + _voted_sensors_update.calc_mag_inconsistency(preflt); orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt); } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 1a240aa1ee..c73dadff34 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -62,6 +62,7 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp)); memset(&_accel_diff, 0, sizeof(_accel_diff)); memset(&_gyro_diff, 0, sizeof(_gyro_diff)); + memset(&_mag_diff, 0, sizeof(_mag_diff)); // initialise the corrections memset(&_corrections, 0, sizeof(_corrections)); @@ -1210,4 +1211,51 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) } } +void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) +{ + float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared + 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 < _mag.subscription_count; sensor_index++) { + + // check that the sensor we are checking against is not the same as the primary + if ((_mag.priority[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) { + + float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary + + // calculate mag_diff_sum_sq for the specified sensor against the primary + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f * + (_last_sensor_data[_mag.last_best_vote].magnetometer_ga[axis_index] - + _last_sensor_data[sensor_index].magnetometer_ga[axis_index]); + mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index]; + + } + + // capture the largest sum value + if (mag_diff_sum_sq > mag_diff_sum_max_sq) { + mag_diff_sum_max_sq = mag_diff_sum_sq; + + } + + // increment the check index + check_index++; + } + + // check to see if the maximum number of checks has been reached and break + if (check_index >= 2) { + break; + + } + } + + // skip check if less than 2 sensors + if (check_index < 1) { + preflt.mag_inconsistency_ga = 0.0f; + + } else { + // get the vector length of the largest difference and write to the combined sensor struct + preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq); + } +} \ No newline at end of file diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 4039c36990..60d6f7cd10 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -142,7 +142,12 @@ public: /** * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor */ - void calc_gyro_inconsistency(struct sensor_preflight_s &preflt); + void calc_gyro_inconsistency(sensor_preflight_s &preflt); + + /** + * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers + */ + void calc_mag_inconsistency(sensor_preflight_s &preflt); private: @@ -263,6 +268,7 @@ private: float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */ float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */ + float _mag_diff[3][2]; /**< filtered mag differences between sensor instances (Ga) */ /* sensor thermal compensation */ TemperatureCompensation _temperature_compensation;