From 45187e1aa8f4f031457476408e6c1b033e742a75 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 26 Jun 2019 13:54:03 +0100 Subject: [PATCH] Improve magnetometer inconsistency check To check directional difference between the magnetometer field vectors instead of vector component difference. --- .../px4fmu_common/init.d/airframes/4250_teal | 2 +- msg/sensor_preflight.msg | 8 ++-- src/modules/commander/PreflightCheck.cpp | 24 +++++----- src/modules/commander/commander_params.c | 11 ++--- src/modules/sensors/sensors.cpp | 2 +- src/modules/sensors/voted_sensors_update.cpp | 48 +++++++------------ src/modules/sensors/voted_sensors_update.h | 4 +- 7 files changed, 43 insertions(+), 56 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index 8629eeea57..f39268089a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -47,7 +47,7 @@ then param set CAL_MAG0_ROT 0 param set CAL_MAG_SIDES 63 param set SENS_BOARD_ROT 0 - param set COM_ARM_MAG 1.5 + param set COM_ARM_MAG_ANG -1 param set COM_ARM_EKF_AB 0.0032 # circuit breakers diff --git a/msg/sensor_preflight.msg b/msg/sensor_preflight.msg index c820595377..1c97183cf7 100644 --- a/msg/sensor_preflight.msg +++ b/msg/sensor_preflight.msg @@ -2,7 +2,7 @@ # Pre-flight sensor check metrics. These will be zero if the vehicle only has one sensor. # The topic will not be updated when the vehicle is armed # -uint64 timestamp # time since system start (microseconds) -float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). -float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). -float32 mag_inconsistency_ga # magnitude of maximum difference between Mag instances in (Gauss). +uint64 timestamp # time since system start (microseconds) +float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). +float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). +float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index b8ad44a1a4..98f18bf891 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -45,6 +45,7 @@ #include "rc_check.h" #include +#include #include #include @@ -203,6 +204,8 @@ out: // return false if the magnetomer measurements are inconsistent static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_status) { + bool pass = false; // flag for result of checks + // get the sensor preflight data uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; sensors_sub.update(); @@ -210,25 +213,24 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s if (sensors.timestamp == 0) { // can happen if not advertised (yet) - return true; + pass = true; } // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. // If a single sensor is fitted, the value being checked will be zero so this check will always pass. - float test_limit; - param_get(param_find("COM_ARM_MAG"), &test_limit); + int32_t angle_difference_limit_deg; + param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg); - if (sensors.mag_inconsistency_ga > test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); - } + pass = pass || angle_difference_limit_deg < 0; // disabled, pass check + pass = pass || sensors.mag_inconsistency_angle < math::radians(angle_difference_limit_deg); - return false; + if (!pass && report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent"); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); } - return true; + return pass; } static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ebe7c74466..f1886b3c47 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -600,15 +600,14 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f); /** * Maximum magnetic field inconsistency between units that will allow arming + * Set -1 to disable the check. * * @group Commander - * @unit Gauss - * @min 0.05 - * @max 0.5 - * @decimal 2 - * @increment 0.05 + * @unit deg + * @min 3 + * @max 180 */ -PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f); +PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 30); /** * Enable RC stick override of auto modes diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5765df93e8..33380cf345 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -673,7 +673,7 @@ Sensors::run() preflt.timestamp = hrt_absolute_time(); _voted_sensors_update.calc_accel_inconsistency(preflt); _voted_sensors_update.calc_gyro_inconsistency(preflt); - _voted_sensors_update.calc_mag_inconsistency(preflt); + _voted_sensors_update.calcMagInconsistency(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 bfa67ba020..c70b87aec7 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -50,6 +50,7 @@ using namespace sensors; using namespace DriverFramework; +using namespace matrix; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) : _parameters(parameters), _hil_enabled(hil_enabled) @@ -60,7 +61,7 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); memset(&_accel_diff, 0, sizeof(_accel_diff)); memset(&_gyro_diff, 0, sizeof(_gyro_diff)); - memset(&_mag_diff, 0, sizeof(_mag_diff)); + memset(&_mag_angle_diff, 0, sizeof(_mag_angle_diff)); // initialise the publication variables memset(&_corrections, 0, sizeof(_corrections)); @@ -1205,34 +1206,25 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) } } -void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) +void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt) { - float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared + Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector + float mag_angle_diff_max = 0.0f; // the maximum angle difference unsigned check_index = 0; // the number of sensors the primary has been checked against // Check each sensor against the primary - for (int sensor_index = 0; sensor_index < _mag.subscription_count; sensor_index++) { - + 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[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) { + if ((_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(); - float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary + // complementary filter to not fail/pass on single outliers + _mag_angle_diff[check_index] *= 0.95f; + _mag_angle_diff[check_index] += 0.05f * angle_error; - // 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_magnetometer[_mag.last_best_vote].magnetometer_ga[axis_index] - - _last_magnetometer[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; - - } + mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); // increment the check index check_index++; @@ -1241,16 +1233,10 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // 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); - } + // get the vector length of the largest difference and write to the combined sensor struct + // will be zero if there is only one magnetometer and hence nothing to compare + preflt.mag_inconsistency_angle = mag_angle_diff_max; } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index d008430fed..c398cec1fb 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -141,7 +141,7 @@ public: /** * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers */ - void calc_mag_inconsistency(sensor_preflight_s &preflt); + void calcMagInconsistency(sensor_preflight_s &preflt); private: @@ -261,7 +261,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) */ + float _mag_angle_diff[2]; /**< filtered mag angle differences between sensor instances (Ga) */ /* sensor thermal compensation */ TemperatureCompensation _temperature_compensation;