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

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

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.