From fe8c896d692a270b7adb3cf6909d8d644d7d7395 Mon Sep 17 00:00:00 2001
From: Adam M Rivera <a432511@gmail.com>
Date: Sat, 10 Mar 2012 17:19:04 -0600
Subject: [PATCH] AP_Declination: Added method set_initial_location

This will set the declination based on lat/lon if the user has
not yet saved one to the EEPROM, OR if they have specified via the
config parameter that they want it to overwrite the declination every
3D fix.

Signed-off-by: Andrew Tridgell <tridge@samba.org>
---
 libraries/AP_Compass/Compass.cpp | 19 +++++++++++++++++++
 libraries/AP_Compass/Compass.h   |  9 +++++++++
 2 files changed, 28 insertions(+)

diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index 7a54c25dc0..ad84d9d03a 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -57,6 +57,25 @@ Compass::get_offsets()
     return _offset;
 }
 
+bool
+Compass::set_initial_location(long latitude, long longitude, bool force)
+{
+	// If the user has choosen to use auto-declination regardless of the planner value
+	// OR
+	// If the declination failed to load from the EEPROM (ie. not set by user)
+	if(force || !_declination.load())
+	{
+		// Set the declination based on the lat/lng from GPS
+		_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
+
+		// Reset null offsets
+		null_offsets_disable();
+		null_offsets_enable();
+		return true;
+	}
+	return false;
+}
+
 void
 Compass::set_declination(float radians)
 {
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 2d423909c0..770a520d60 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -5,6 +5,7 @@
 #include <inttypes.h>
 #include "../AP_Common/AP_Common.h"
 #include "../AP_Math/AP_Math.h"
+#include "../AP_Declination/AP_Declination.h" // ArduPilot Mega Declination Helper Library
 
 // compass product id
 #define AP_COMPASS_TYPE_UNKNOWN  0x00
@@ -80,6 +81,14 @@ public:
 	///
 	virtual Vector3f &get_offsets();
 
+	/// Sets the initial location used to get declination - Returns true if declination set
+	///
+	/// @param  latitude             GPS Latitude.
+	/// @param  longitude            GPS Longitude.
+	/// @param  force				 Force the compass declination update.
+	///
+	bool set_initial_location(long latitude, long longitude, bool force);
+
 	/// Program new offset values.
 	///
 	/// @param  x                   Offset to the raw mag_x value.