From 9e6fcded41928867d442088af1d56d1a5b58af45 Mon Sep 17 00:00:00 2001
From: jjulio1234 <jjulio1234@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Wed, 10 Nov 2010 21:38:56 +0000
Subject: [PATCH] Geographic_correction_factor is now internally calculated (no
 need for external parameter)

git-svn-id: https://arducopter.googlecode.com/svn/trunk@822 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArducopterNG/Arducopter.h     |  6 +++---
 ArducopterNG/ArducopterNG.pde |  2 ++
 ArducopterNG/Navigation.pde   | 11 +++++++----
 3 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h
index a8164258ef..b2f36042f8 100644
--- a/ArducopterNG/Arducopter.h
+++ b/ArducopterNG/Arducopter.h
@@ -484,7 +484,7 @@ float Kp_ROLLPITCH;
 float Ki_ROLLPITCH;
 float Kp_YAW;
 float Ki_YAW;
-float GEOG_CORRECTION_FACTOR;
+float GEOG_CORRECTION_FACTOR=0;
 int MAGNETOMETER;
 float Kp_RateRoll;
 float Ki_RateRoll;
@@ -550,7 +550,7 @@ void defaultUserConfig() {
   Ki_ROLLPITCH               = 0.00000015;
   Kp_YAW                     = 1.0;
   Ki_YAW                     = 0.00002;
-  GEOG_CORRECTION_FACTOR     = 0.87;
+  GEOG_CORRECTION_FACTOR     = 0.0;      // will be automatically calculated
   MAGNETOMETER               = 0;
   Kp_RateRoll                = 1.95;
   Ki_RateRoll                = 0.0;
@@ -656,4 +656,4 @@ void defaultUserConfig() {
 
 
 
-// end of file
+// end of file
diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde
index 0bb10fe1b8..9a7dece1ae 100644
--- a/ArducopterNG/ArducopterNG.pde
+++ b/ArducopterNG/ArducopterNG.pde
@@ -210,6 +210,8 @@ void setup() {
   mediumLoop = mainLoop;
   GPS_timer = mainLoop;
   motorArmed = 0;
+  
+  GEOG_CORRECTION_FACTOR = 0;   // Geographic correction factor will be automatically calculated
 
   Read_adc_raw();            // Initialize ADC readings...
 
diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde
index ca434d3ac4..46ae02eaf4 100644
--- a/ArducopterNG/Navigation.pde
+++ b/ArducopterNG/Navigation.pde
@@ -58,12 +58,15 @@ void Position_control(long lat_dest, long lon_dest)
   long Lon_diff;
   long Lat_diff;
 
-#ifdef IsGPS   // it is safe to disable this, if no IsGPS we never come here anyways
   Lon_diff = lon_dest - GPS.Longitude;
   Lat_diff = lat_dest - GPS.Lattitude;
-#endif
+
+  //If we have not calculated GEOG_CORRECTION_FACTOR we calculate it here as cos(lattitude)
+  if (GEOG_CORRECTION_FACTOR==0)
+    GEOG_CORRECTION_FACTOR = cos(ToRad(GPS.Lattitude/10000000.0));
+
   // ROLL
-  //Optimization : cos(yaw) = DCM_Matrix[0][0] ;  sin(yaw) = DCM_Matrix[1][0] 
+  //Optimization : cos(yaw) = DCM_Matrix[0][0] ;  sin(yaw) = DCM_Matrix[1][0]   [This simplification is valid for low roll angles]
   gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
 
   gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
@@ -99,4 +102,4 @@ void Reset_I_terms_navigation()
   gps_pitch_I = 0;  
 }
 
-
+