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