dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: add pre-flight check and parameter for magnetic field strength

This commit is contained in:
bresch 2019-11-11 14:57:06 +01:00 committed by Mathieu Bresciani
parent a8ea55d9b6
commit 12177cb33b
4 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/estimator_status.msg
View File

@ -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)

6
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
View File

@ -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");
}
}

7
src/modules/ekf2/ekf2_main.cpp
View File

@ -522,7 +522,8 @@ private:
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h ///< Required GPS health time
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _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);

14
src/modules/ekf2/ekf2_params.c
View File

@ -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);