From 2ca48037fd717673fa0c6c9ceba759acd8856de7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 13 Feb 2016 19:11:02 +1100
Subject: [PATCH] ekf2: Add mavlink adjustable parameters for control of
 magnetometer fusion

---
 src/modules/ekf2/ekf2_main.cpp |  5 +++++
 src/modules/ekf2/ekf2_params.c | 26 ++++++++++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 5aa9a39018..2a4d8bd484 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -175,6 +175,9 @@ private:
 	control::BlockParamFloat *_mag_declination_deg;	// magnetic declination in degrees
 	control::BlockParamFloat *_heading_innov_gate;	// innovation gate for heading innovation test
 	control::BlockParamFloat *_mag_innov_gate;	// innovation gate for magnetometer innovation test
+	control::BlockParamInt
+	*_mag_decl_source;       // bitmasked integer used to control the handling of magnetic declination
+	control::BlockParamInt *_mag_fuse_type;         // integer ued to control the type of magnetometer fusion used
 
 	control::BlockParamInt *_gps_check_mask;        // bitmasked integer used to activate the different GPS quality checks
 	control::BlockParamFloat *_requiredEph;         // maximum acceptable horiz position error (m)
@@ -225,6 +228,8 @@ Ekf2::Ekf2():
 	_mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)),
 	_heading_innov_gate(new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate)),
 	_mag_innov_gate(new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate)),
+	_mag_decl_source(new control::BlockParamInt(this, "EKF2_DECL_TYPE", false, &_params->mag_declination_source)),
+	_mag_fuse_type(new control::BlockParamInt(this, "EKF2_MAG_TYPE", false, &_params->mag_fusion_type)),
 	_gps_check_mask(new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask)),
 	_requiredEph(new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &_params->req_hacc)),
 	_requiredEpv(new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &_params->req_vacc)),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 5c4a43d5d7..c1dc9b86f1 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -326,6 +326,32 @@ PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
 
+/**
+ * Integer bitmask controlling handling of magnetic declination. Set bits to in the following positions to enable functions.
+ * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value.
+ * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms.
+ * 2 : Set to true to always use the declination as an observaton when 3-axis magnetometer fusion is being used.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 7
+ * @unit
+ */
+PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
+
+/**
+ * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-axis magnetometer.
+ * 0 : determine the best fusion method to use automatically - heading fusion on-ground and 3-axis fusion in-flight
+ * 1 : always use magnetic heading fusion
+ * 2 : always use 3-axis fusion
+ * Other values  will disable magnetometer fusion completely
+ * @group EKF2
+ * @min 0
+ * @max 2
+ * @unit None
+ */
+PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
+
 /**
  * Gate size for barometric height fusion
  *