From faa2ab18f64009affd85872a08639ce2c2ae26eb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 19 Jun 2019 12:19:28 +1000 Subject: [PATCH 1/3] msg: rework mag inconsistency reporting messages --- msg/sensor_preflight.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/sensor_preflight.msg b/msg/sensor_preflight.msg index c82059537708..f2bcff5bae86 100644 --- a/msg/sensor_preflight.msg +++ b/msg/sensor_preflight.msg @@ -5,4 +5,5 @@ 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). +float32 mag_inconsistency_h # magnitude of maximum horizontal angular difference between Mag instances in (rad). +float32 mag_inconsistency_v # magnitude of maximum vertical (inclination) angular difference between Mag instances in (rad). From 98d3d1073a5d2f4fe75b4aec2ab8d8fbcb9fd23b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 19 Jun 2019 12:20:09 +1000 Subject: [PATCH 2/3] Commander: Rework preflight mag consistency checks Check horizontal and vertical angular discrepancy separately. --- src/modules/commander/PreflightCheck.cpp | 9 ++- src/modules/commander/commander_params.c | 22 +++++-- src/modules/sensors/voted_sensors_update.cpp | 60 +++++++++++++++----- src/modules/sensors/voted_sensors_update.h | 2 +- 4 files changed, 70 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4bab7c8e512c..b2a4fd014a40 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -220,10 +220,13 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s // 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); + float test_limit_horiz; + param_get(param_find("COM_ARM_MAG_H"), &test_limit_horiz); - if (sensors.mag_inconsistency_ga > test_limit) { + float test_limit_vert; + param_get(param_find("COM_ARM_MAG_V"), &test_limit_vert); + + if ((sensors.mag_inconsistency_h > test_limit_horiz) || (sensors.mag_inconsistency_v > test_limit_vert)) { 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); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0bb1a5bf2a15..e59603d114ef 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -599,16 +599,28 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_ACC, 0.7f); PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f); /** - * Maximum magnetic field inconsistency between units that will allow arming + * Maximum magnetic field horizontal angle inconsistency between units that will allow arming. Values greater than Pi effectively disable this check. * * @group Commander - * @unit Gauss - * @min 0.05 - * @max 0.5 + * @unit rad + * @min 0.1 + * @max 4.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_MAG_H, 0.5f); + +/** + * Maximum magnetic field inclination angle inconsistency between units that will allow arming. Values greater than Pi effectively disable this check. + * + * @group Commander + * @unit rad + * @min 0.1 + * @max 4.0 * @decimal 2 * @increment 0.05 */ -PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f); +PARAM_DEFINE_FLOAT(COM_ARM_MAG_V, 0.7f); /** * Enable RC stick override of auto modes diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 4ddc4a3ed229..de6d4294ed2d 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -60,7 +60,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)); @@ -1209,7 +1209,8 @@ 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 + float mag_h_diff_max = 0.0f; // the maximum horizontal field angle difference (rad) + float mag_v_diff_max = 0.0f; // the maximum vertical field angle difference (rad) unsigned check_index = 0; // the number of sensors the primary has been checked against // Check each sensor against the primary @@ -1218,21 +1219,50 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // 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 + // Get unit gravity vector in body frame (assume platform is not accelerating) + matrix::Vector3f gravity_vec(-_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[0], + -_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[1], + -_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[2]); + gravity_vec.normalize(); - // 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]); + // calculate the cross product for each sensor with assumed gravity vector to produce a horizontal projection + // could use estimator body to earth rotation matrix to do this, but that would require subscription to + // a new topic and is unnecessary for quasi-static preflight checks. + matrix::Vector3f mag_vec_selected(_last_magnetometer[_mag.last_best_vote].magnetometer_ga[0], + _last_magnetometer[_mag.last_best_vote].magnetometer_ga[1], + _last_magnetometer[_mag.last_best_vote].magnetometer_ga[2]); - mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index]; + matrix::Vector3f mag_vec_alternate(_last_magnetometer[sensor_index].magnetometer_ga[0], + _last_magnetometer[sensor_index].magnetometer_ga[1], + _last_magnetometer[sensor_index].magnetometer_ga[2]); - } + matrix::Vector3f mag_hvec_selected = gravity_vec.cross(mag_vec_selected); + matrix::Vector3f mag_hvec_alternate = gravity_vec.cross(mag_vec_alternate); + + // calculate the dot product for each sensor with the unit gravity vector to get the vertical component + float mag_down_selected = gravity_vec.dot(mag_vec_selected); + float mag_down_alternate = gravity_vec.dot(mag_vec_alternate); + + // calculate the vertical angle between the two vectors + float mag_vang_selected = atan2f(mag_down_selected, mag_hvec_selected.norm()); + float mag_vang_alternate = atan2f(mag_down_alternate, mag_hvec_alternate.norm()); + float ang_v_delta = fabsf(mag_vang_selected - mag_vang_alternate); + + // calculate the horizontal angle between the two vectors to estimate the difference in magnetic heading thee sensors would produce + matrix::Quatf q_delta(mag_hvec_selected, mag_hvec_alternate); + matrix::Vector3f ang_hvec_delta = q_delta.to_axis_angle(); + float ang_h_delta = ang_hvec_delta.norm(); + + _mag_angle_diff[0][check_index] = 0.95f * _mag_angle_diff[0][check_index] + 0.05f * ang_h_delta; + _mag_angle_diff[1][check_index] = 0.95f * _mag_angle_diff[1][check_index] + 0.05f * ang_v_delta; // capture the largest sum value - if (mag_diff_sum_sq > mag_diff_sum_max_sq) { - mag_diff_sum_max_sq = mag_diff_sum_sq; + if (fabsf(_mag_angle_diff[0][check_index]) > mag_h_diff_max) { + mag_h_diff_max = fabsf(_mag_angle_diff[0][check_index]); + + } + if (fabsf(_mag_angle_diff[1][check_index]) > mag_v_diff_max) { + mag_v_diff_max = fabsf(_mag_angle_diff[1][check_index]); } @@ -1249,10 +1279,12 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // skip check if less than 2 sensors if (check_index < 1) { - preflt.mag_inconsistency_ga = 0.0f; + preflt.mag_inconsistency_h = 0.0f; + preflt.mag_inconsistency_v = 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); + preflt.mag_inconsistency_h = mag_h_diff_max; + preflt.mag_inconsistency_v = mag_v_diff_max; } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index f905690506eb..3e4762693352 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -260,7 +260,7 @@ class VotedSensorsUpdate 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][2]; /**< filtered magnetic declination and inclination differences between sensor instances (rad) */ /* sensor thermal compensation */ TemperatureCompensation _temperature_compensation; From a7683e59354b68616be8e4427570fcfbf2b9b617 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 19 Jun 2019 14:51:07 +1000 Subject: [PATCH 3/3] ROMFS: ensure mag consistency check remains disabled for 4250_teal airframe --- ROMFS/px4fmu_common/init.d/airframes/4250_teal | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index 8629eeea57c9..33431ccd6fe7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -47,7 +47,8 @@ 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_H 4.0 + param set COM_ARM_MAG_V 4.0 param set COM_ARM_EKF_AB 0.0032 # circuit breakers