diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 41d7f17e6a..5bb501f3dd 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -214,6 +214,32 @@ out: return success; } +// return false if the magnetomer measurements are inconsistent +static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status) +{ + // get the sensor preflight data + int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight)); + struct sensor_preflight_s sensors = {}; + if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) { + // can happen if not advertised (yet) + return true; + } + orb_unsubscribe(sensors_sub); + + // 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); + if (sensors.mag_inconsistency_ga > test_limit) { + if (report_status) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT"); + } + return false; + } + + return true; +} + static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -625,6 +651,12 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, } failed = true; } + + /* fail if mag sensors are inconsistent */ + if (!magConsistencyCheck(mavlink_log_pub, reportFailures)) { + failed = true; + } + } /* ---- ACCEL ---- */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index bd9b5ba354..319453e790 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -590,6 +590,18 @@ 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 + * + * @group Commander + * @unit Gauss + * @min 0.05 + * @max 0.5 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f); + /** * Enable RC stick override of auto modes *