From af5bbc916ce9fdc99e7b47360d0ba916befd8aa3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 13 Feb 2016 19:12:47 +1100
Subject: [PATCH] ekf2: Enable user defeatible saving of declination for next
 startup

---
 src/modules/ekf2/ekf2_main.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 2a4d8bd484..8b4075cd24 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -131,6 +131,8 @@ private:
 	int		_control_mode_sub = -1;
 	int 	_vehicle_status_sub = -1;
 
+	bool            _prev_motors_armed = false; // motors armed status from the previous frame
+
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
@@ -563,6 +565,15 @@ void Ekf2::task_main()
 			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
 		}
 
+		// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
+		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
+			float decl_deg;
+			_ekf->copy_mag_decl_deg(&decl_deg);
+			param_set(param_find("EKF2_MAG_DECL"), &decl_deg);
+		}
+
+		_prev_motors_armed = vehicle_control_mode.flag_armed;
+
 	}
 
 	delete ekf2::instance;