From edb627a5c17fd0e43010b64dd4c6e6cd364b0da4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 30 Mar 2012 13:38:32 +1100
Subject: [PATCH] AP_Declination: added timing information to declination test

---
 .../AP_Declination_test.pde                   | 58 +++++++++++--------
 1 file changed, 33 insertions(+), 25 deletions(-)

diff --git a/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde b/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
index ff46cd09a..cc74e2de2 100644
--- a/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
+++ b/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
@@ -6,6 +6,8 @@
 #include <AP_Math.h>
 #include <AP_Declination.h>
 #include <Filter.h>
+#include <I2C.h>
+#include <SPI.h>
 
 #ifdef DESKTOP_BUILD
 // all of this is needed to build with SITL
@@ -66,10 +68,37 @@ static const int16_t dec_tbl[37][73] = \
 {168,173,178,176,171,166,161,156,151,146,141,136,131,126,121,116,111,106,101,-96,-91,-86,-81,-76,-71,-66,-61,-56,-51,-46,-41,-36,-31,-26,-21,-16,-11,-6 ,-1 ,3  ,8  ,13 ,18 ,23 ,28 ,33 ,38 ,43 ,48 ,53 ,58 ,63 ,68 ,73 ,78 ,83 ,88 ,93 ,98 ,103,108,113,118,123,128,133,138,143,148,153,158,163,168}, \
 };
 
+static float get_declination(float lat, float lon)
+{
+	int16_t decSW, decSE, decNW, decNE, lonmin, latmin;
+	float decmin, decmax;
+	uint8_t latmin_index, lonmin_index;
+
+	// Validate input values
+	lat = constrain(lat, -90, 90);
+	lon = constrain(lon, -180, 180);
+
+	latmin = floor(lat/5)*5;
+	lonmin = floor(lon/5)*5;
+
+	latmin_index= (90+latmin)/5;
+	lonmin_index= (180+lonmin)/5;
+
+	decSW = dec_tbl[latmin_index][lonmin_index];
+	decSE = dec_tbl[latmin_index][lonmin_index+1];
+	decNE = dec_tbl[latmin_index+1][lonmin_index+1];
+	decNW = dec_tbl[latmin_index+1][lonmin_index];
+
+	decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW;
+	decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW;
+	return   (lat - latmin) / 5 * (decmax - decmin) + decmin;
+}
+
 void setup(void)
 {
 	float declination, declination_test;
 	uint16_t pass = 0, fail = 0;
+    uint32_t total_time=0;
 
 	Serial.begin(115200);
 	Serial.print("Beginning Test. Please wait...\n");
@@ -78,7 +107,9 @@ void setup(void)
 	{
 		for(int16_t j = -180; j <= 180; j+=5)
 		{
+            uint32_t t1 = micros();
 			declination = AP_Declination::get_declination(i, j);
+            total_time += micros() - t1;
 			declination_test = get_declination(i, j);
 			if(declination == declination_test)
 			{
@@ -95,34 +126,11 @@ void setup(void)
 	Serial.print("Ending Test.\n\n");
 	Serial.printf("Total Pass: %i\n", pass);
 	Serial.printf("Total Fail: %i\n", fail);
+    Serial.printf("Average time per call: %.1f usec\n",
+                  total_time/(float)(pass+fail));
 }
 
 void loop(void)
 {
 }
 
-float get_declination(float lat, float lon)
-{
-	int16_t decSW, decSE, decNW, decNE, lonmin, latmin;
-	float decmin, decmax;
-	uint8_t latmin_index, lonmin_index;
-
-	// Validate input values
-	lat = constrain(lat, -90, 90);
-	lon = constrain(lon, -180, 180);
-
-	latmin = floor(lat/5)*5;
-	lonmin = floor(lon/5)*5;
-
-	latmin_index= (90+latmin)/5;
-	lonmin_index= (180+lonmin)/5;
-
-	decSW = dec_tbl[latmin_index][lonmin_index];
-	decSE = dec_tbl[latmin_index][lonmin_index+1];
-	decNE = dec_tbl[latmin_index+1][lonmin_index+1];
-	decNW = dec_tbl[latmin_index+1][lonmin_index];
-
-	decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW;
-	decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW;
-	return   (lat - latmin) / 5 * (decmax - decmin) + decmin;
-}
-- 
GitLab