From 12177cb33b2661bfc41bda713cd8530253da0eef Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 11 Nov 2019 14:57:06 +0100 Subject: [PATCH] commander: add pre-flight check and parameter for magnetic field strength --- msg/estimator_status.msg | 1 + .../Arming/PreFlightCheck/checks/ekf2Check.cpp | 6 +++++- src/modules/ekf2/ekf2_main.cpp | 7 +++++-- src/modules/ekf2/ekf2_params.c | 14 ++++++++++++++ 4 files changed, 25 insertions(+), 3 deletions(-) diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 44754f5fdd..7c0a1869ea 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -110,6 +110,7 @@ bool pre_flt_fail_innov_heading bool pre_flt_fail_innov_vel_horiz bool pre_flt_fail_innov_vel_vert bool pre_flt_fail_innov_height +bool pre_flt_fail_mag_field_disturbed # legacy local position estimator (LPE) flags uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index c9ee73d098..e6cd17fc1f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -65,7 +65,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & if (status.pre_flt_fail_innov_heading || status.pre_flt_fail_innov_vel_horiz || status.pre_flt_fail_innov_vel_vert || - status.pre_flt_fail_innov_height) { + status.pre_flt_fail_innov_height || + status.pre_flt_fail_mag_field_disturbed) { if (report_fail) { if (status.pre_flt_fail_innov_heading) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable"); @@ -78,6 +79,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } else if (status.pre_flt_fail_innov_height) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable"); + + } else if (status.pre_flt_fail_mag_field_disturbed) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected"); } } diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index da3f9ee319..9da92a588d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -522,7 +522,8 @@ private: (ParamExtFloat) _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. - (ParamFloat) _param_ekf2_req_gps_h ///< Required GPS health time + (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time + (ParamExtInt) _param_ekf2_mag_check ///< Mag field strength check ) @@ -627,7 +628,8 @@ Ekf2::Ekf2(bool replay_mode): _param_ekf2_drag_noise(_params->drag_noise), _param_ekf2_bcoef_x(_params->bcoef_x), _param_ekf2_bcoef_y(_params->bcoef_y), - _param_ekf2_move_test(_params->is_moving_scaler) + _param_ekf2_move_test(_params->is_moving_scaler), + _param_ekf2_mag_check(_params->check_mag_strength) { // initialise parameter cache updateParams(); @@ -1541,6 +1543,7 @@ void Ekf2::Run() status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); + status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; _estimator_status_pub.publish(status); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index d9360f9582..c48b4df1c4 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1403,3 +1403,17 @@ PARAM_DEFINE_FLOAT(EKF2_MOVE_TEST, 1.0f); * @reboot_required true */ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); + +/** + * Magnetic field strength test selection + * + * When set, the EKF checks the strength of the magnetic field + * to decide whether the magnetometer data is valid. + * If GPS data is received, the magnetic field is compared to a World + * Magnetic Model (WMM), otherwise an average value is used. + * This check is useful to reject occasional hard iron disturbance. + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0);