Skip to content
Snippets Groups Projects
Commit edb627a5 authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

AP_Declination: added timing information to declination test

parent 01c4cde1
No related branches found
No related tags found
No related merge requests found
...@@ -6,6 +6,8 @@ ...@@ -6,6 +6,8 @@
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Declination.h> #include <AP_Declination.h>
#include <Filter.h> #include <Filter.h>
#include <I2C.h>
#include <SPI.h>
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
// all of this is needed to build with SITL // all of this is needed to build with SITL
...@@ -66,10 +68,37 @@ static const int16_t dec_tbl[37][73] = \ ...@@ -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}, \ {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) void setup(void)
{ {
float declination, declination_test; float declination, declination_test;
uint16_t pass = 0, fail = 0; uint16_t pass = 0, fail = 0;
uint32_t total_time=0;
Serial.begin(115200); Serial.begin(115200);
Serial.print("Beginning Test. Please wait...\n"); Serial.print("Beginning Test. Please wait...\n");
...@@ -78,7 +107,9 @@ void setup(void) ...@@ -78,7 +107,9 @@ void setup(void)
{ {
for(int16_t j = -180; j <= 180; j+=5) for(int16_t j = -180; j <= 180; j+=5)
{ {
uint32_t t1 = micros();
declination = AP_Declination::get_declination(i, j); declination = AP_Declination::get_declination(i, j);
total_time += micros() - t1;
declination_test = get_declination(i, j); declination_test = get_declination(i, j);
if(declination == declination_test) if(declination == declination_test)
{ {
...@@ -95,34 +126,11 @@ void setup(void) ...@@ -95,34 +126,11 @@ void setup(void)
Serial.print("Ending Test.\n\n"); Serial.print("Ending Test.\n\n");
Serial.printf("Total Pass: %i\n", pass); Serial.printf("Total Pass: %i\n", pass);
Serial.printf("Total Fail: %i\n", fail); Serial.printf("Total Fail: %i\n", fail);
Serial.printf("Average time per call: %.1f usec\n",
total_time/(float)(pass+fail));
} }
void loop(void) 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;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment