zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Declination: Implemented Delta Encoding and Run-Length Encoding. Added method to traverse the compressed array and return the lookup value based on the same lat_index/lon_index that was used before.

mission-4.1.18
Adam M Rivera 13 years ago committed by Andrew Tridgell
parent
3a849771bc
commit
0dcc4e8307
2 changed files with 121 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 161
      libraries/AP_Declination/AP_Declination.cpp
  2. 2
      libraries/AP_Declination/AP_Declination.h

161
libraries/AP_Declination/AP_Declination.cpp
Unescape View File

@ -20,44 +20,83 @@ @@ -20,44 +20,83 @@
#include <avr/pgmspace.h>
#include <math.h>
static const int16_t dec_tbl[37][73] PROGMEM = \
{{150,145,140,135,130,125,120,115,110,105,100,95,90,85,80,75,70,65,60,55,50,45,40,35,30,25,20,15,10,5,0,-4,-9,-14,-19,-24,-29,-34,-39,-44,-49,-54,-59,-64,-69,-74,-79,-84,-89,-94,-99,104,109,114,119,124,129,134,139,144,149,154,159,164,169,174,179,175,170,165,160,155,150}, \
{143,137,131,126,120,115,110,105,100,95,90,85,80,75,71,66,62,57,53,48,44,39,35,31,27,22,18,14,9,5,1,-3,-7,-11,-16,-20,-25,-29,-34,-38,-43,-47,-52,-57,-61,-66,-71,-76,-81,-86,-91,-96,101,107,112,117,123,128,134,140,146,151,157,163,169,175,178,172,166,160,154,148,143}, \
{130,124,118,112,107,101,96,92,87,82,78,74,70,65,61,57,54,50,46,42,38,34,31,27,23,19,16,12,8,4,1,-2,-6,-10,-14,-18,-22,-26,-30,-34,-38,-43,-47,-51,-56,-61,-65,-70,-75,-79,-84,-89,-94,100,105,111,116,122,128,135,141,148,155,162,170,177,174,166,159,151,144,137,130}, \
{111,104,99,94,89,85,81,77,73,70,66,63,60,56,53,50,46,43,40,36,33,30,26,23,20,16,13,10,6,3,0,-3,-6,-9,-13,-16,-20,-24,-28,-32,-36,-40,-44,-48,-52,-57,-61,-65,-70,-74,-79,-84,-88,-93,-98,103,109,115,121,128,135,143,152,162,172,176,165,154,144,134,125,118,111}, \
{85,81,77,74,71,68,65,63,60,58,56,53,51,49,46,43,41,38,35,32,29,26,23,19,16,13,10,7,4,1,-1,-3,-6,-9,-13,-16,-19,-23,-26,-30,-34,-38,-42,-46,-50,-54,-58,-62,-66,-70,-74,-78,-83,-87,-91,-95,100,105,110,117,124,133,144,159,178,160,141,125,112,103,96,90,85}, \
{62,60,58,57,55,54,52,51,50,48,47,46,44,42,41,39,36,34,31,28,25,22,19,16,13,10,7,4,2,0,-3,-5,-8,-10,-13,-16,-19,-22,-26,-29,-33,-37,-41,-45,-49,-53,-56,-60,-64,-67,-70,-74,-77,-80,-83,-86,-89,-91,-94,-97,101,105,111,130,109,84,77,74,71,68,66,64,62}, \
{46,46,45,44,44,43,42,42,41,41,40,39,38,37,36,35,33,31,28,26,23,20,16,13,10,7,4,1,-1,-3,-5,-7,-9,-12,-14,-16,-19,-22,-26,-29,-33,-36,-40,-44,-48,-51,-55,-58,-61,-64,-66,-68,-71,-72,-74,-74,-75,-74,-72,-68,-61,-48,-25,2,22,33,40,43,45,46,47,46,46}, \
{36,36,36,36,36,35,35,35,35,34,34,34,34,33,32,31,30,28,26,23,20,17,14,10,6,3,0,-2,-4,-7,-9,-10,-12,-14,-15,-17,-20,-23,-26,-29,-32,-36,-40,-43,-47,-50,-53,-56,-58,-60,-62,-63,-64,-64,-63,-62,-59,-55,-49,-41,-30,-17,-4,6,15,22,27,31,33,34,35,36,36}, \
{30,30,30,30,30,30,30,29,29,29,29,29,29,29,29,28,27,26,24,21,18,15,11,7,3,0,-3,-6,-9,-11,-12,-14,-15,-16,-17,-19,-21,-23,-26,-29,-32,-35,-39,-42,-45,-48,-51,-53,-55,-56,-57,-57,-56,-55,-53,-49,-44,-38,-31,-23,-14,-6,0,7,13,17,21,24,26,27,29,29,30}, \
{25,25,26,26,26,25,25,25,25,25,25,25,25,26,25,25,24,23,21,19,16,12,8,4,0,-3,-7,-10,-13,-15,-16,-17,-18,-19,-20,-21,-22,-23,-25,-28,-31,-34,-37,-40,-43,-46,-48,-49,-50,-51,-51,-50,-48,-45,-42,-37,-32,-26,-19,-13,-7,-1,3,7,11,14,17,19,21,23,24,25,25}, \
{21,22,22,22,22,22,22,22,22,22,22,22,22,22,22,22,21,20,18,16,13,9,5,1,-3,-7,-11,-14,-17,-18,-20,-21,-21,-22,-22,-22,-23,-23,-25,-27,-29,-32,-35,-37,-40,-42,-44,-45,-45,-45,-44,-42,-40,-36,-32,-27,-22,-17,-12,-7,-3,0,3,7,9,12,14,16,18,19,20,21,21}, \
{18,19,19,19,19,19,19,19,19,19,19,19,19,19,19,19,18,17,16,14,10,7,2,-1,-6,-10,-14,-17,-19,-21,-22,-23,-24,-24,-24,-24,-23,-23,-23,-24,-26,-28,-30,-33,-35,-37,-38,-39,-39,-38,-36,-34,-31,-28,-24,-19,-15,-10,-6,-3,0,1,4,6,8,10,12,14,15,16,17,18,18}, \
{16,16,17,17,17,17,17,17,17,17,17,16,16,16,16,16,16,15,13,11,8,4,0,-4,-9,-13,-16,-19,-21,-23,-24,-25,-25,-25,-25,-24,-23,-21,-20,-20,-21,-22,-24,-26,-28,-30,-31,-32,-31,-30,-29,-27,-24,-21,-17,-13,-9,-6,-3,-1,0,2,4,5,7,9,10,12,13,14,15,16,16}, \
{14,14,14,15,15,15,15,15,15,15,14,14,14,14,14,14,13,12,11,9,5,2,-2,-6,-11,-15,-18,-21,-23,-24,-25,-25,-25,-25,-24,-22,-21,-18,-16,-15,-15,-15,-17,-19,-21,-22,-24,-24,-24,-23,-22,-20,-18,-15,-12,-9,-5,-3,-1,0,1,2,4,5,6,8,9,10,11,12,13,14,14}, \
{12,13,13,13,13,13,13,13,13,13,13,13,12,12,12,12,11,10,9,6,3,0,-4,-8,-12,-16,-19,-21,-23,-24,-24,-24,-24,-23,-22,-20,-17,-15,-12,-10,-9,-9,-10,-12,-13,-15,-17,-17,-18,-17,-16,-15,-13,-11,-8,-5,-3,-1,0,1,1,2,3,4,6,7,8,9,10,11,12,12,12}, \
{11,11,11,11,11,12,12,12,12,12,11,11,11,11,11,10,10,9,7,5,2,-1,-5,-9,-13,-17,-20,-22,-23,-23,-23,-23,-22,-20,-18,-16,-14,-11,-9,-6,-5,-4,-5,-6,-8,-9,-11,-12,-12,-12,-12,-11,-9,-8,-6,-3,-1,0,0,1,1,2,3,4,5,6,7,8,9,10,11,11,11}, \
{10,10,10,10,10,10,10,10,10,10,10,10,10,10,9,9,9,7,6,3,0,-3,-6,-10,-14,-17,-20,-21,-22,-22,-22,-21,-19,-17,-15,-13,-10,-8,-6,-4,-2,-2,-2,-2,-4,-5,-7,-8,-8,-9,-8,-8,-7,-5,-4,-2,0,0,1,1,1,2,2,3,4,5,6,7,8,9,10,10,10},
{9,9,9,9,9,9,9,10,10,9,9,9,9,9,9,8,8,6,5,2,0,-4,-7,-11,-15,-17,-19,-21,-21,-21,-20,-18,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,0,-1,-2,-4,-5,-5,-6,-6,-5,-5,-4,-3,-1,0,0,1,1,1,1,2,3,3,5,6,7,8,8,9,9,9}, \
{9,9,9,9,9,9,9,9,9,9,9,9,8,8,8,8,7,5,4,1,-1,-5,-8,-12,-15,-17,-19,-20,-20,-19,-18,-16,-14,-11,-9,-7,-5,-4,-2,-1,0,0,1,1,0,0,-2,-3,-3,-4,-4,-4,-3,-3,-2,-1,0,0,0,0,0,1,1,2,3,4,5,6,7,8,8,9,9}, \
{9,9,9,8,8,8,9,9,9,9,9,8,8,8,8,7,6,5,3,0,-2,-5,-9,-12,-15,-17,-18,-19,-19,-18,-16,-14,-12,-9,-7,-5,-4,-2,-1,0,0,1,1,1,1,0,0,-1,-2,-2,-3,-3,-2,-2,-1,-1,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8,9}, \
{8,8,8,8,8,8,9,9,9,9,9,9,8,8,8,7,6,4,2,0,-3,-6,-9,-12,-15,-17,-18,-18,-17,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,2,2,1,0,0,-1,-1,-1,-2,-2,-1,-1,0,0,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8}, \
{8,8,8,8,9,9,9,9,9,9,9,9,9,8,8,7,5,3,1,-1,-4,-7,-10,-13,-15,-16,-17,-17,-16,-15,-13,-11,-9,-6,-5,-3,-2,0,0,0,1,2,2,2,2,1,1,0,0,0,-1,-1,-1,-1,-1,0,0,0,0,-1,-1,-1,-1,-1,0,0,1,3,4,5,7,7,8}, \
{8,8,9,9,9,9,10,10,10,10,10,10,10,9,8,7,5,3,0,-2,-5,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,3,3,2,2,1,0,0,0,0,0,0,0,0,0,0,-1,-1,-2,-2,-2,-2,-2,-1,0,0,1,3,4,6,7,8}, \
{7,8,9,9,9,10,10,11,11,11,11,11,10,10,9,7,5,3,0,-2,-6,-9,-11,-13,-15,-16,-16,-15,-14,-13,-11,-9,-7,-5,-3,-2,0,0,1,1,2,3,3,3,3,2,2,1,1,0,0,0,0,0,0,0,-1,-1,-2,-3,-3,-4,-4,-4,-3,-2,-1,0,1,3,5,6,7}, \
{6,8,9,9,10,11,11,12,12,12,12,12,11,11,9,7,5,2,0,-3,-7,-10,-12,-14,-15,-16,-15,-15,-13,-12,-10,-8,-7,-5,-3,-1,0,0,1,2,2,3,3,4,3,3,3,2,2,1,1,1,0,0,0,0,-1,-2,-3,-4,-4,-5,-5,-5,-5,-4,-2,-1,0,2,3,5,6}, \
{6,7,8,10,11,12,12,13,13,14,14,13,13,11,10,8,5,2,0,-4,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-5,-3,-1,0,0,1,2,3,3,4,4,4,4,4,3,3,3,2,2,1,1,0,0,-1,-2,-3,-5,-6,-7,-7,-7,-6,-5,-4,-3,-1,0,2,4,6}, \
{5,7,8,10,11,12,13,14,15,15,15,14,14,12,11,8,5,2,-1,-5,-9,-12,-14,-16,-17,-17,-16,-15,-14,-12,-11,-9,-7,-5,-3,-1,0,0,1,2,3,4,4,5,5,5,5,5,5,4,4,3,3,2,1,0,-1,-2,-4,-6,-7,-8,-8,-8,-8,-7,-6,-4,-2,0,1,3,5}, \
{4,6,8,10,12,13,14,15,16,16,16,16,15,13,11,9,5,2,-2,-6,-10,-13,-16,-17,-18,-18,-17,-16,-15,-13,-11,-9,-7,-5,-4,-2,0,0,1,3,3,4,5,6,6,7,7,7,7,7,6,5,4,3,2,0,-1,-3,-5,-7,-8,-9,-10,-10,-10,-9,-7,-5,-4,-1,0,2,4}, \
{4,6,8,10,12,14,15,16,17,18,18,17,16,15,12,9,5,1,-3,-8,-12,-15,-18,-19,-20,-20,-19,-18,-16,-15,-13,-11,-8,-6,-4,-2,-1,0,1,3,4,5,6,7,8,9,9,9,9,9,9,8,7,5,3,1,-1,-3,-6,-8,-10,-11,-12,-12,-11,-10,-9,-7,-5,-2,0,1,4}, \
{4,6,8,11,13,15,16,18,19,19,19,19,18,16,13,10,5,0,-5,-10,-15,-18,-21,-22,-23,-22,-22,-20,-18,-17,-14,-12,-10,-8,-5,-3,-1,0,1,3,5,6,8,9,10,11,12,12,13,12,12,11,9,7,5,2,0,-3,-6,-9,-11,-12,-13,-13,-12,-11,-10,-8,-6,-3,-1,1,4}, \
{3,6,9,11,14,16,17,19,20,21,21,21,19,17,14,10,4,-1,-8,-14,-19,-22,-25,-26,-26,-26,-25,-23,-21,-19,-17,-14,-12,-9,-7,-4,-2,0,1,3,5,7,9,11,13,14,15,16,16,16,16,15,13,10,7,4,0,-3,-7,-10,-12,-14,-15,-14,-14,-12,-11,-9,-6,-4,-1,1,3}, \
{4,6,9,12,14,17,19,21,22,23,23,23,21,19,15,9,2,-5,-13,-20,-25,-28,-30,-31,-31,-30,-29,-27,-25,-22,-20,-17,-14,-11,-9,-6,-3,0,1,4,6,9,11,13,15,17,19,20,21,21,21,20,18,15,11,6,2,-2,-7,-11,-13,-15,-16,-16,-15,-13,-11,-9,-7,-4,-1,1,4}, \
{4,7,10,13,15,18,20,22,24,25,25,25,23,20,15,7,-2,-12,-22,-29,-34,-37,-38,-38,-37,-36,-34,-31,-29,-26,-23,-20,-17,-13,-10,-7,-4,-1,2,5,8,11,13,16,18,21,23,24,26,26,26,26,24,21,17,12,5,0,-6,-10,-14,-16,-16,-16,-15,-14,-12,-10,-7,-4,-1,1,4}, \
{4,7,10,13,16,19,22,24,26,27,27,26,24,19,11,-1,-15,-28,-37,-43,-46,-47,-47,-45,-44,-41,-39,-36,-32,-29,-26,-22,-19,-15,-11,-8,-4,-1,2,5,9,12,15,19,22,24,27,29,31,33,33,33,32,30,26,21,14,6,0,-6,-11,-14,-15,-16,-15,-14,-12,-9,-7,-4,-1,1,4}, \
{6,9,12,15,18,21,23,25,27,28,27,24,17,4,-14,-34,-49,-56,-60,-60,-60,-58,-56,-53,-50,-47,-43,-40,-36,-32,-28,-25,-21,-17,-13,-9,-5,-1,2,6,10,14,17,21,24,28,31,34,37,39,41,42,43,43,41,38,33,25,17,8,0,-4,-8,-10,-10,-10,-8,-7,-4,-2,0,3,6}, \
{22,24,26,28,30,32,33,31,23,-18,-81,-96,-99,-98,-95,-93,-89,-86,-82,-78,-74,-70,-66,-62,-57,-53,-49,-44,-40,-36,-32,-27,-23,-19,-14,-10,-6,-1,2,6,10,15,19,23,27,31,35,38,42,45,49,52,55,57,60,61,63,63,62,61,57,53,47,40,33,28,23,21,19,19,19,20,22},
{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}};
// 2 bytes - 9 bits for value + 5 bits for repeats => 14 padded to 16
struct row_value{
// Offset has a max value of 203 and a min value of -197
// If we take the abs value of each offset and store its sign in a separate bit
// we can save a lot of wasted 8 bits and simplify reading the offsets
uint8_t abs_offset;
// Sign of the offset, 0 = negative, 1 = positive
uint8_t offset_sign:1;
// The highest repeat is 29
uint8_t repeats:5;
};
// 76 bytes - 8 bits per 37 row_start + 8 bits per 37 row_length
struct row_keys{
// The highest row_start is 168
uint8_t row_start[37];
// The highest length is 54 which fits in 6 bits,
// but the struct will pad to the nearest byte anyway
// so there is no savings by specifying length
uint8_t row_length[37];
};
// 76 bytes - Instance of the struct defined above
// I decided NOT to store this in PROGMEM because it is small and the expense of pulling
// the value out of the PROGMEM is too high
static const row_keys declination_keys = \
{ \
{150,143,130,111,85,62,46,36,30,25,21,18,16,14,12,11,10,9,9,9,8,8,8,7,6,6,5,4,4,4,3,4,4,4,6,22,168}, \
{7,39,39,39,36,39,46,39,37,33,34,36,35,37,35,38,33,40,41,41,28,38,37,41,50,48,37,35,33,43,49,48,47,54,44,46,7}, \
};
// 1409 total values @ 2 bytes each = 2818 bytes
static const row_value declination_values[] PROGMEM = \
{ \
{0,1,0},{5,0,29},{4,0,0},{5,0,18},{203,1,0},{5,1,14},{4,0,0}, \
{0,1,0},{6,0,1},{5,0,0},{6,0,0},{5,0,8},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,2},{5,0,0},{4,0,1},{5,0,0},{4,0,4},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{5,0,1},{4,0,0},{5,0,6},{197,1,0},{6,1,0},{5,1,1},{6,1,0},{5,1,0},{6,1,2},{5,1,0},{6,1,3},{3,1,0},{6,0,4}, \
{0,1,0},{6,0,2},{5,0,0},{6,0,0},{5,0,0},{4,0,0},{5,0,1},{4,0,2},{5,0,0},{4,0,1},{3,0,0},{4,0,4},{3,0,0},{4,0,2},{3,0,0},{4,0,2},{3,0,1},{4,0,8},{5,0,0},{4,0,1},{5,0,1},{4,0,0},{5,0,1},{4,0,0},{5,0,2},{194,1,0},{5,1,0},{6,1,0},{5,1,0},{6,1,1},{7,1,0},{6,1,0},{7,1,2},{8,1,0},{7,1,0},{3,0,0},{8,0,0},{7,0,0},{8,0,0}, \
{0,1,0},{7,0,0},{5,0,2},{4,0,3},{3,0,0},{4,0,0},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,4},{4,0,0},{3,0,0},{4,0,8},{5,0,0},{4,0,1},{5,0,0},{4,0,0},{5,0,1},{4,0,0},{5,0,1},{201,1,0},{6,1,2},{7,1,1},{8,1,0},{9,1,0},{10,1,1},{4,1,0},{11,0,1},{10,0,1},{9,0,0}, \
{0,1,0},{4,0,1},{3,0,3},{2,0,0},{3,0,0},{2,0,1},{3,0,0},{2,0,1},{3,0,1},{2,0,0},{3,0,5},{4,0,0},{3,0,5},{2,0,1},{3,0,1},{4,0,0},{3,0,1},{4,0,0},{3,0,0},{4,0,12},{5,0,0},{4,0,2},{195,1,0},{5,1,1},{7,1,1},{9,1,0},{11,1,0},{15,1,0},{19,1,0},{18,0,0},{19,0,0},{16,0,0},{13,0,0},{9,0,0},{7,0,0},{6,0,0}, \
{0,1,0},{2,0,1},{1,0,0},{2,0,0},{1,0,0},{2,0,0},{1,0,1},{2,0,0},{1,0,1},{2,0,1},{1,0,0},{2,0,0},{3,0,0},{2,0,0},{3,0,9},{2,0,1},{3,0,0},{2,0,0},{3,0,0},{2,0,0},{3,0,3},{4,0,0},{3,0,0},{4,0,5},{3,0,0},{4,0,1},{3,0,1},{4,0,0},{3,0,4},{2,0,0},{3,0,1},{198,1,0},{4,1,0},{6,1,0},{19,1,0},{21,0,0},{25,0,0},{7,0,0},{3,0,2}, \
{0,1,1},{1,0,1},{0,1,0},{1,0,1},{0,1,0},{1,0,0},{0,1,0},{1,0,5},{2,0,1},{3,0,0},{2,0,0},{3,0,1},{4,0,0},{3,0,4},{2,0,4},{3,0,0},{2,0,1},{3,0,1},{4,0,0},{3,0,0},{4,0,0},{3,0,0},{4,0,2},{3,0,0},{4,0,0},{3,0,2},{2,0,1},{3,0,0},{1,0,0},{2,0,0},{0,1,0},{1,0,0},{1,1,0},{2,1,0},{4,1,0},{7,1,0},{13,1,0},{23,1,0},{27,1,0},{20,1,0},{11,1,0},{7,1,0},{3,1,0},{2,1,0},{1,1,1},{1,0,0}, \
{0,1,4},{1,0,0},{0,1,2},{1,0,0},{0,1,2},{1,0,3},{2,0,1},{3,0,3},{4,0,1},{3,0,1},{2,0,1},{3,0,0},{2,0,0},{1,0,0},{2,0,1},{1,0,0},{2,0,0},{3,0,4},{4,0,1},{3,0,0},{4,0,0},{3,0,2},{2,0,2},{1,0,1},{0,1,0},{1,1,1},{3,1,0},{4,1,0},{6,1,0},{8,1,0},{11,1,0},{13,1,1},{10,1,0},{9,1,0},{7,1,0},{5,1,0},{4,1,0},{2,1,0},{1,1,2}, \
{0,1,6},{1,0,0},{0,1,6},{1,0,2},{2,0,0},{3,0,2},{4,0,2},{3,0,3},{2,0,0},{1,0,0},{2,0,0},{1,0,2},{2,0,2},{3,0,3},{4,0,0},{3,0,3},{2,0,1},{1,0,1},{0,1,0},{1,1,1},{2,1,0},{4,1,0},{5,1,0},{6,1,0},{7,1,0},{8,1,0},{9,1,0},{8,1,0},{6,1,0},{7,1,0},{6,1,0},{4,1,1},{3,1,0},{2,1,0},{1,1,0},{2,1,0},{0,1,0}, \
{0,1,1},{1,1,0},{0,1,1},{1,0,0},{0,1,6},{1,1,0},{1,0,0},{0,1,0},{1,0,1},{2,0,1},{3,0,0},{4,0,3},{3,0,0},{4,0,0},{3,0,1},{2,0,0},{1,0,7},{2,0,0},{3,0,6},{2,0,0},{1,0,2},{0,1,0},{1,1,0},{2,1,0},{3,1,1},{5,1,1},{6,1,0},{7,1,0},{6,1,2},{4,1,2},{3,1,1},{2,1,2},{1,1,1}, \
{0,1,0},{1,1,0},{0,1,13},{1,0,1},{2,0,1},{3,0,0},{4,0,5},{3,0,1},{1,0,0},{2,0,0},{1,0,0},{0,1,0},{1,0,0},{0,1,1},{1,0,0},{0,1,0},{2,0,2},{3,0,1},{2,0,0},{3,0,0},{2,0,1},{1,0,0},{0,1,1},{1,1,0},{2,1,1},{4,1,1},{5,1,4},{4,1,0},{3,1,1},{4,1,0},{2,1,0},{3,1,0},{2,1,2},{1,1,2}, \
{0,1,0},{1,1,0},{0,1,13},{1,0,2},{2,0,0},{4,0,0},{3,0,0},{5,0,0},{3,0,0},{5,0,0},{4,0,1},{3,0,0},{2,0,1},{1,0,2},{0,1,2},{1,1,0},{0,1,1},{1,0,0},{2,0,2},{3,0,0},{2,0,1},{1,0,1},{0,1,0},{1,1,0},{2,1,1},{3,1,1},{4,1,0},{5,1,0},{4,1,0},{5,1,0},{4,1,0},{3,1,1},{1,1,0},{3,1,0},{2,1,4},{1,1,3}, \
{0,1,1},{1,1,0},{0,1,7},{1,0,0},{0,1,4},{1,0,0},{2,0,1},{3,0,0},{4,0,2},{5,0,0},{4,0,0},{3,0,1},{2,0,1},{1,0,1},{0,1,2},{1,1,1},{2,1,0},{1,1,0},{0,1,0},{1,0,1},{2,0,3},{1,0,1},{1,1,2},{2,1,0},{3,1,1},{4,1,2},{3,1,1},{2,1,0},{1,1,0},{2,1,1},{1,1,0},{2,1,1},{1,1,0},{2,1,0},{1,1,3}, \
{0,1,2},{1,1,0},{0,1,5},{1,0,0},{0,1,4},{1,0,2},{2,0,0},{4,0,0},{3,0,0},{4,0,1},{5,0,0},{4,0,0},{3,0,1},{2,0,0},{1,0,1},{0,1,2},{1,1,0},{2,1,0},{1,1,0},{3,1,0},{2,1,0},{1,1,0},{0,1,1},{2,0,2},{1,0,0},{2,0,0},{0,1,1},{1,1,1},{2,1,1},{3,1,2},{4,1,0},{2,1,1},{1,1,2},{2,1,0},{1,1,1},{2,1,0},{1,1,5}, \
{0,1,0},{1,1,0},{0,1,9},{1,0,0},{0,1,2},{1,0,2},{3,0,2},{4,0,3},{3,0,0},{2,0,1},{1,0,0},{0,1,2},{1,1,1},{2,1,0},{3,1,0},{2,1,0},{3,1,0},{2,1,0},{1,1,0},{0,1,0},{1,0,0},{2,0,0},{1,0,0},{2,0,1},{0,1,0},{1,0,0},{1,1,2},{2,1,1},{3,1,1},{2,1,1},{1,1,1},{0,1,0},{1,1,2},{2,1,0},{1,1,5}, \
{0,1,4},{1,1,0},{0,1,3},{1,0,0},{0,1,3},{1,0,0},{0,1,0},{1,0,0},{2,0,1},{3,0,1},{4,0,3},{3,0,0},{2,0,0},{1,0,0},{0,1,2},{1,1,0},{2,1,3},{3,1,0},{2,1,0},{3,1,0},{1,1,1},{1,0,1},{2,0,0},{1,0,0},{2,0,0},{1,0,0},{0,1,2},{1,1,0},{2,1,0},{1,1,0},{2,1,0},{3,1,0},{2,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,0},{1,1,9}, \
{0,1,13},{1,0,0},{0,1,1},{2,0,0},{1,0,0},{3,0,3},{4,0,1},{3,0,1},{1,0,1},{0,1,1},{1,1,0},{2,1,3},{3,1,0},{2,1,3},{0,1,2},{2,0,0},{1,0,0},{2,0,0},{1,0,0},{0,1,0},{1,0,0},{1,1,0},{0,1,0},{1,1,0},{2,1,0},{1,1,0},{2,1,1},{0,1,0},{1,1,0},{0,1,1},{1,1,0},{0,1,0},{1,1,7}, \
{0,1,6},{1,1,0},{0,1,0},{1,0,0},{0,1,4},{1,0,0},{0,1,0},{2,0,0},{1,0,0},{3,0,0},{2,0,0},{4,0,0},{3,0,0},{4,0,1},{2,0,2},{0,1,1},{1,1,0},{2,1,8},{1,1,1},{0,1,1},{1,0,1},{2,0,0},{1,0,0},{0,1,0},{1,0,0},{0,1,0},{1,1,0},{0,1,0},{1,1,1},{2,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,2},{1,1,1},{0,1,0},{2,1,0},{1,1,2},{0,1,0},{1,1,0}, \
{0,1,11},{1,0,0},{0,1,2},{1,0,0},{2,0,0},{1,0,0},{3,0,0},{2,0,0},{4,0,0},{3,0,0},{4,0,0},{3,0,0},{2,0,1},{1,0,0},{0,1,0},{1,1,1},{2,1,1},{3,1,0},{2,1,2},{1,1,0},{2,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,0},{1,0,0},{0,1,0},{2,0,0},{1,0,0},{0,1,0},{1,0,0},{0,1,1},{1,1,0},{0,1,0},{1,1,2},{0,1,3},{1,1,0},{0,1,0},{1,1,6},{0,1,0},{1,1,0}, \
{0,1,2},{1,0,0},{0,1,1},{1,1,0},{0,1,3},{1,0,0},{0,1,2},{1,0,2},{2,0,0},{3,0,0},{2,0,0},{3,0,0},{4,0,0},{3,0,1},{2,0,0},{1,0,1},{0,1,0},{1,1,0},{2,1,2},{3,1,0},{2,1,1},{1,1,0},{2,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,2},{1,0,0},{0,1,0},{1,0,1},{0,1,0},{1,0,0},{0,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,5},{1,1,7},{0,1,0}, \
{0,1,5},{1,1,0},{0,1,4},{1,0,0},{0,1,1},{1,0,1},{2,0,2},{3,0,4},{2,0,0},{1,0,0},{0,1,0},{1,1,1},{2,1,6},{1,1,1},{0,1,0},{1,1,1},{0,1,2},{1,0,1},{0,1,0},{1,0,0},{0,1,1},{1,0,0},{0,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,7},{1,1,7}, \
{0,1,3},{1,1,0},{0,1,7},{1,0,0},{0,1,0},{1,0,0},{2,0,3},{3,0,3},{2,0,0},{1,0,1},{0,1,0},{1,1,1},{2,1,2},{3,1,0},{1,1,0},{2,1,0},{1,1,0},{2,1,0},{0,1,1},{1,1,1},{0,1,2},{1,0,0},{0,1,0},{1,0,0},{0,1,1},{1,0,0},{0,1,3},{1,1,0},{0,1,2},{1,0,0},{0,1,3},{1,1,0},{0,1,0},{1,1,0},{2,1,0},{1,1,1},{2,1,0},{0,1,0}, \
{0,1,1},{1,1,0},{0,1,2},{1,1,0},{0,1,5},{1,0,2},{2,0,1},{3,0,0},{2,0,0},{3,0,2},{2,0,1},{1,0,0},{0,1,1},{1,1,0},{2,1,0},{1,1,0},{2,1,4},{1,1,1},{0,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,0},{1,0,0},{0,1,0},{1,0,1},{0,1,8},{1,0,0},{0,1,0},{1,0,0},{0,1,3},{1,1,1},{0,1,0},{1,1,0},{2,1,0},{1,1,0},{2,1,0}, \
{0,1,0},{1,1,1},{0,1,1},{1,1,0},{0,1,0},{1,1,0},{0,1,3},{1,0,0},{0,1,0},{1,0,0},{2,0,2},{3,0,0},{2,0,0},{4,0,0},{3,0,0},{2,0,2},{1,0,0},{0,1,0},{1,1,2},{2,1,4},{1,1,0},{2,1,0},{0,1,0},{1,1,0},{0,1,0},{1,1,1},{0,1,2},{1,0,0},{0,1,0},{1,0,0},{0,1,0},{1,0,0},{0,1,5},{1,0,0},{0,1,0},{1,0,1},{0,1,0},{1,0,0},{0,1,1},{1,1,4},{2,1,1}, \
{0,1,0},{2,1,0},{1,1,0},{0,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,3},{1,0,0},{0,1,0},{2,0,2},{3,0,0},{2,0,0},{3,0,0},{4,0,0},{3,0,0},{2,0,1},{1,0,1},{1,1,0},{0,1,0},{2,1,0},{1,1,0},{2,1,1},{1,1,0},{2,1,2},{1,1,0},{0,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,0},{1,1,0},{1,0,0},{0,1,1},{1,0,0},{0,1,0},{1,0,0},{0,1,1},{1,0,0},{0,1,2},{1,0,3},{0,1,0},{1,0,0},{0,1,2},{1,1,0},{2,1,0},{1,1,1},{2,1,0},{1,1,0},{2,1,0}, \
{0,1,0},{1,1,1},{2,1,0},{1,1,1},{0,1,0},{1,1,0},{0,1,0},{1,1,0},{0,1,0},{1,0,0},{0,1,0},{2,0,0},{1,0,0},{2,0,0},{3,0,1},{2,0,0},{4,0,1},{3,0,0},{2,0,1},{1,0,0},{0,1,1},{1,1,0},{2,1,0},{1,1,0},{2,1,2},{1,1,0},{2,1,1},{1,1,0},{0,1,0},{1,1,2},{0,1,0},{1,1,0},{0,1,3},{1,0,0},{0,1,1},{1,0,0},{0,1,0},{1,0,0},{0,1,0},{1,0,0},{0,1,0},{1,0,2},{2,0,0},{1,0,1},{0,1,1},{1,1,3},{2,1,0},{1,1,0}, \
{0,1,0},{2,1,0},{1,1,0},{2,1,0},{1,1,4},{0,1,1},{1,0,0},{0,1,0},{2,0,0},{1,0,0},{3,0,3},{4,0,1},{3,0,0},{2,0,1},{1,0,0},{0,1,0},{1,1,2},{2,1,0},{1,1,0},{2,1,4},{1,1,0},{0,1,0},{1,1,3},{0,1,0},{1,1,0},{0,1,4},{1,0,0},{0,1,0},{1,0,0},{0,1,0},{1,0,4},{2,0,1},{1,0,1},{0,1,2},{1,1,1},{2,1,2},{1,1,0}, \
{0,1,0},{2,1,3},{1,1,3},{0,1,2},{1,0,0},{2,0,2},{4,0,0},{3,0,0},{4,0,2},{3,0,1},{1,0,1},{0,1,0},{1,1,2},{2,1,4},{1,1,0},{2,1,1},{0,1,0},{1,1,0},{2,1,0},{0,1,0},{1,1,2},{0,1,0},{1,1,0},{0,1,3},{1,0,4},{2,0,0},{1,0,0},{2,0,2},{1,0,2},{0,1,1},{1,1,0},{2,1,1},{1,1,0},{3,1,0},{1,1,0}, \
{0,1,0},{2,1,4},{1,1,3},{0,1,0},{1,0,2},{3,0,1},{4,0,2},{5,0,0},{4,0,0},{3,0,1},{1,0,1},{0,1,0},{1,1,1},{2,1,0},{1,1,0},{2,1,1},{3,1,0},{2,1,2},{1,1,2},{2,1,0},{1,1,5},{0,1,4},{1,0,1},{2,0,4},{3,0,0},{2,0,1},{1,0,1},{0,1,0},{1,1,2},{2,1,1},{3,1,0},{2,1,0},{1,1,0}, \
{0,1,0},{2,1,1},{3,1,0},{2,1,1},{1,1,0},{2,1,0},{1,1,0},{0,1,2},{1,0,0},{2,0,0},{3,0,1},{5,0,4},{3,0,1},{1,0,1},{1,1,0},{0,1,0},{2,1,1},{1,1,0},{3,1,0},{2,1,2},{3,1,0},{2,1,1},{1,1,1},{2,1,1},{1,1,0},{2,1,0},{1,1,3},{0,1,0},{1,1,0},{1,0,0},{0,1,0},{1,0,0},{2,0,2},{3,0,0},{2,0,0},{3,0,2},{2,0,0},{1,0,1},{0,1,0},{1,1,2},{2,1,1},{3,1,0},{2,1,1}, \
{0,1,0},{3,1,1},{2,1,0},{3,1,0},{2,1,0},{1,1,0},{2,1,0},{1,1,1},{0,1,1},{2,0,1},{3,0,0},{4,0,0},{6,0,0},{5,0,0},{7,0,0},{6,0,0},{5,0,0},{3,0,1},{1,0,0},{0,1,1},{1,1,0},{2,1,3},{3,1,0},{2,1,0},{3,1,0},{2,1,0},{3,1,0},{2,1,1},{1,1,0},{2,1,5},{1,1,2},{0,1,2},{1,0,0},{2,0,0},{3,0,2},{4,0,0},{3,0,0},{4,0,0},{3,0,0},{2,0,1},{1,0,0},{1,1,0},{0,1,0},{2,1,0},{1,1,0},{2,1,0},{3,1,0},{2,1,0},{3,1,0}, \
{0,1,0},{2,1,0},{3,1,1},{2,1,0},{3,1,0},{2,1,1},{1,1,1},{0,1,1},{2,0,1},{4,0,0},{6,0,0},{7,0,1},{8,0,0},{7,0,0},{5,0,0},{3,0,0},{2,0,0},{1,0,0},{0,1,0},{1,1,1},{2,1,1},{3,1,0},{2,1,0},{3,1,2},{2,1,0},{3,1,2},{1,1,0},{3,1,0},{2,1,0},{3,1,0},{2,1,4},{1,1,1},{0,1,1},{1,0,0},{2,0,0},{3,0,0},{4,0,0},{5,0,0},{4,0,1},{5,0,0},{4,0,0},{2,0,1},{1,0,0},{0,1,0},{1,1,0},{2,1,3},{3,1,1},{2,1,0}, \
{0,1,0},{3,1,2},{2,1,0},{3,1,0},{2,1,2},{1,1,0},{0,1,1},{2,0,0},{3,0,0},{5,0,0},{8,0,0},{9,0,0},{10,0,1},{7,0,0},{5,0,0},{3,0,0},{1,0,0},{0,1,0},{1,1,1},{2,1,0},{3,1,0},{2,1,0},{3,1,3},{4,1,0},{3,1,7},{2,1,0},{3,1,0},{2,1,0},{3,1,0},{2,1,0},{1,1,0},{2,1,0},{0,1,2},{2,0,0},{3,0,0},{4,0,0},{5,0,0},{7,0,0},{5,0,0},{6,0,0},{4,0,1},{2,0,0},{0,1,1},{1,1,1},{2,1,1},{3,1,2},{2,1,0}, \
{0,1,0},{3,1,5},{2,1,1},{1,1,0},{0,1,0},{1,0,0},{2,0,0},{5,0,0},{8,0,0},{12,0,0},{14,0,0},{13,0,0},{9,0,0},{6,0,0},{3,0,0},{1,0,0},{0,1,0},{2,1,0},{1,1,0},{3,1,0},{2,1,0},{3,1,0},{4,1,0},{3,1,1},{4,1,0},{3,1,0},{4,1,1},{3,1,0},{4,1,0},{3,1,2},{4,1,0},{3,1,1},{4,1,0},{3,1,0},{2,1,0},{3,1,0},{2,1,2},{0,1,1},{1,0,0},{2,0,0},{4,0,0},{5,0,0},{7,0,0},{8,0,0},{6,0,1},{5,0,0},{3,0,0},{1,0,1},{1,1,1},{2,1,0},{3,1,0},{2,1,0},{3,1,1},{2,1,0}, \
{0,1,0},{3,1,4},{2,1,2},{1,1,0},{1,0,0},{3,0,0},{7,0,0},{13,0,0},{18,0,0},{20,0,0},{15,0,0},{7,0,0},{4,0,0},{0,1,1},{2,1,1},{3,1,2},{4,1,0},{3,1,0},{4,1,2},{3,1,0},{4,1,5},{3,1,0},{4,1,2},{3,1,0},{4,1,0},{3,1,0},{4,1,0},{3,1,2},{2,1,1},{1,1,1},{0,1,0},{2,0,0},{3,0,0},{5,0,0},{8,0,1},{9,0,0},{8,0,0},{4,0,1},{2,0,0},{0,1,1},{2,1,0},{1,1,0},{3,1,0},{2,1,1}, \
{0,1,0},{2,1,4},{1,1,0},{2,0,0},{8,0,0},{41,0,0},{63,0,0},{15,0,0},{3,0,0},{1,1,0},{3,1,0},{2,1,0},{4,1,0},{3,1,0},{4,1,5},{5,1,0},{4,1,1},{5,1,0},{4,1,2},{5,1,0},{4,1,1},{5,1,0},{4,1,1},{5,1,0},{3,1,0},{4,1,1},{5,1,0},{4,1,4},{3,1,0},{4,1,0},{3,1,0},{4,1,0},{3,1,1},{2,1,0},{3,1,0},{1,1,0},{2,1,0},{0,1,0},{1,0,1},{4,0,1},{6,0,0},{7,0,1},{5,0,1},{2,0,1},{0,1,1},{1,1,0}, \
{0,1,0},{5,1,1},{2,0,0},{5,0,14},{197,0,0},{5,1,18},{4,1,0}, \
};
float
AP_Declination::get_declination(float lat, float lon)
@ -76,13 +115,51 @@ AP_Declination::get_declination(float lat, float lon) @@ -76,13 +115,51 @@ AP_Declination::get_declination(float lat, float lon)
latmin_index= (90+latmin)/5;
lonmin_index= (180+lonmin)/5;
decSW = (int16_t)pgm_read_word_far(&dec_tbl[latmin_index][lonmin_index]);
decSE = (int16_t)pgm_read_word_far(&dec_tbl[latmin_index][lonmin_index+1]);
decNE = (int16_t)pgm_read_word_far(&dec_tbl[latmin_index+1][lonmin_index+1]);
decNW = (int16_t)pgm_read_word_far(&dec_tbl[latmin_index+1][lonmin_index]);
decSW = get_lookup_value(latmin_index, lonmin_index);
decSE = get_lookup_value(latmin_index, lonmin_index+1);
decNE = get_lookup_value(latmin_index+1, lonmin_index+1);
decNW = get_lookup_value(latmin_index+1, lonmin_index);
/* approximate declination within the grid using bilinear interpolation */
decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW;
decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW;
return (lat - latmin) / 5 * (decmax - decmin) + decmin;
}
int16_t
AP_Declination::get_lookup_value(uint8_t x, uint8_t y)
{
// If we are looking for the first value we can just use the
// row_start value from declination_keys
if(y == 0) return declination_keys.row_start[x];
row_value stval;
int16_t val = declination_keys.row_start[x], offset = 0;
uint8_t current_virtual_index = 0, r;
uint16_t start_index = 0, i;
// Find the first element in the 1D array
// that corresponds with the target row
for(i = 0; i < x; i++){
start_index += declination_keys.row_length[i];
}
// Traverse the row until we find our value
for(i = start_index; i < (start_index + declination_keys.row_length[x]) && current_virtual_index <= y; i++){
// Pull out the row_value struct
memcpy_P((void*) &stval, (void *) &declination_values[i], sizeof(struct row_value));
// Pull the first offset and determine sign
offset = (stval.offset_sign == 1) ? stval.abs_offset : -stval.abs_offset;
// Add offset for each repeat
// This will at least run once for zero repeat
for(r = 0; r <= stval.repeats && current_virtual_index <= y; r++){
val += offset;
current_virtual_index++;
}
}
return val;
}

2
libraries/AP_Declination/AP_Declination.h
Unescape View File

@ -16,6 +16,8 @@ class AP_Declination @@ -16,6 +16,8 @@ class AP_Declination
{
public:
static float get_declination(float lat, float lon);
private:
static int16_t get_lookup_value(uint8_t x, uint8_t y);
};
#endif // AP_Declination_h

Write Preview
Loading…
Cancel
Save