Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-20 15:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity
782c1aaaa3
ardupilot/libraries/AP_Declination/AP_Declination.cpp

165 lines
15 KiB
C++
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Adam M Rivera
* With direction from: Andrew Tridgell, Jason Short, Justin Beech
*
* Adapted from: http://www.societyofrobots.com/robotforum/index.php?topic=11855.0
* Scott Ferguson
* scottfromscott@gmail.com
*
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Declination.h>
#include <avr/pgmspace.h>
#include <math.h>
// 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)
{
int16_t decSW, decSE, decNW, decNE, lonmin, latmin;
uint8_t latmin_index,lonmin_index;
float decmin, decmax;
// Constrain to valid inputs
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 = 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;
}
Reference in New Issue View Git Blame Copy Permalink