You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
302 lines
9.3 KiB
302 lines
9.3 KiB
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "wmm.h"
|
|
|
|
#define PI_CONST 3.14159265359f
|
|
#define RADIANS_TO_DEGREES 0.017453292f
|
|
#define DEGREES_TO_RADIANS (PI_CONST / 180.0f)
|
|
#define A_CONST 6378.137f
|
|
#define A2_CONST (A_CONST * A_CONST)
|
|
#define B_CONST 6356.7523142f
|
|
#define B2_CONST (B_CONST * B_CONST)
|
|
#define RE_CONST 6371.2f
|
|
#define A4_CONST (A2_CONST * A2_CONST)
|
|
#define B4_CONST (B2_CONST * B2_CONST)
|
|
#define C2_CONST (A2_CONST - B2_CONST)
|
|
#define C4_CONST (A4_CONST - B4_CONST)
|
|
#define COEFFICIENTS_COUNT 90U
|
|
|
|
static float c[13][13];
|
|
static float cd[13][13];
|
|
static float k[13][13];
|
|
static float snorm[169];
|
|
static float fn[13];
|
|
static float fm[13];
|
|
|
|
const uint8_t wmm_cof_entries_encoded[] =
|
|
{0xDD, 0xF2, 0x23, 0x00, 0x83, 0x01, 0x00, 0xEB, 0xE2, 0x01, 0x81, 0xD7, 0x05, 0x8D, 0x01, 0xFB, 0x03, 0xE8, 0x86, 0x03, 0x00, 0xF3, 0x01, 0x00, 0xBC, 0xD1, 0x03, 0xDC, 0xD3, 0x03, 0xC7, 0x01, 0xEE, 0x04, 0x80, 0x86, 0x02, 0xF4, 0x72, 0x56, 0xEF, 0x03, 0x87, 0xD5, 0x01, 0x00, 0x1C, 0x00, 0xC2, 0xF4, 0x02, 0xF6, 0x0C, 0x7E, 0x39, 0x8A, 0xC1, 0x01, 0xB2, 0x25, 0x22, 0x4A, 0x89, 0x52, 0xF5, 0x54, 0xFA, 0x01, 0x0B, 0x87, 0x8D, 0x01, 0x00, 0x4B, 0x00, 0x9E, 0x7E, 0x84, 0x2C, 0x50, 0x02, 0x9E, 0x0D, 0xF0, 0x18, 0x7C, 0x85, 0x01, 0xD6, 0x30, 0x8E, 0x1F, 0x36, 0x25, 0x9F, 0x07, 0xED, 0x36, 0x77, 0x78, 0xE8, 0x24, 0x00, 0x43, 0x00, 0xAF, 0x38, 0x9D, 0x07, 0x06, 0x01, 0x96, 0x1D, 0xA4, 0x20, 0x47, 0x19, 0xFF, 0x15, 0xFD, 0x12, 0x01, 0x49, 0xE8, 0x17, 0x82, 0x05, 0x0C, 0x1E, 0x89, 0x02, 0x9F, 0x0F, 0x0A, 0x05, 0x93, 0x0A, 0x00, 0x46, 0x00, 0x90, 0x0A, 0xFF, 0x02, 0x44, 0x01, 0x9A, 0x0B, 0xBA, 0x03, 0x05, 0x52, 0xFF, 0x12, 0x8F, 0x08, 0x0E, 0x4E, 0xEA, 0x05, 0xC4, 0x0A, 0x4E, 0x09, 0x87, 0x02, 0x9A, 0x01, 0x00, 0x01, 0xC7, 0x0A, 0xA9, 0x0A, 0x08, 0x0A, 0xA6, 0x0C, 0x00, 0x41, 0x00, 0xC0, 0x0C, 0xC2, 0x08, 0x43, 0x05, 0xD3, 0x01, 0xE8, 0x02, 0x41, 0x06, 0xB5, 0x08, 0x17, 0x07, 0x47, 0x9E, 0x02, 0xAB, 0x03, 0x02, 0x42, 0x80, 0x01, 0x56, 0x45, 0x4C, 0xC8, 0x01, 0xD0, 0x04, 0x48, 0x02, 0xA2, 0x01, 0x53, 0x0A, 0x03, 0xAC, 0x03, 0x00, 0x41, 0x00, 0xA2, 0x01, 0x94, 0x01, 0x01, 0x43, 0xEF, 0x02, 0xD9, 0x02, 0x41, 0x07, 0x44, 0x80, 0x02, 0x05, 0x42, 0xD3, 0x03, 0xF6, 0x01, 0x41, 0x05, 0x99, 0x02, 0x95, 0x02, 0x04, 0x43, 0x89, 0x02, 0x24, 0x05, 0x45, 0xE5, 0x02, 0xC5, 0x01, 0x00, 0x04, 0x43, 0x1C, 0x04, 0x01, 0x32, 0x00, 0x41, 0x00, 0x92, 0x01, 0xE9, 0x03, 0x42, 0x43, 0x1D, 0xAF, 0x01, 0x00, 0x02, 0x4E, 0xA2, 0x01, 0x04, 0x44, 0x4B, 0x73, 0x43, 0x04, 0xC5, 0x02, 0x7E, 0x00, 0x01, 0x0B, 0x8E, 0x01, 0x03, 0x00, 0x99, 0x01, 0x04, 0x00, 0x42, 0xDD, 0x01, 0x4F, 0x00, 0x05, 0xF7, 0x01, 0xA1, 0x01, 0x44, 0x02, 0x53, 0x00, 0x00, 0x00, 0x7E, 0x22, 0x00, 0x00, 0x41, 0x42, 0x00, 0x01, 0x11, 0x23, 0x02, 0x43, 0x49, 0x30, 0x41, 0x01, 0x06, 0xD6, 0x01, 0x42, 0x42, 0x49, 0x41, 0x00, 0x01, 0x13, 0x6A, 0x41, 0x00, 0x0E, 0x62, 0x42, 0x41, 0x58, 0x41, 0x41, 0x02, 0x67, 0xD8, 0x01, 0x00, 0x00, 0x1E, 0x00, 0x00, 0x00, 0x4E, 0x00, 0x41, 0x00, 0x59, 0x1A, 0x00, 0x01, 0x18, 0x45, 0x00, 0x00, 0x49, 0x44, 0x00, 0x02, 0x03, 0x06, 0x41, 0x00, 0x47, 0x42, 0x00, 0x00, 0x41, 0x51, 0x00, 0x01, 0x0E, 0x50, 0x41, 0x00, 0x46, 0x5E, 0x41, 0x41, 0x02, 0x54, 0x41, 0x00, 0x1F, 0x5A, 0x41, 0x00, 0x54, 0x00, 0x00, 0x00, 0x41, 0x4C, 0x00, 0x00, 0x05, 0x05, 0x00, 0x00, 0x0D, 0x0D, 0x00, 0x41, 0x4C, 0x52, 0x00, 0x01, 0x07, 0x01, 0x00, 0x00, 0x03, 0x07, 0x00, 0x00, 0x05, 0x41, 0x00, 0x00, 0x42, 0x06, 0x00, 0x01, 0x45, 0x02, 0x00, 0x00, 0x01, 0x49, 0x00, 0x00, 0x4B, 0x00, 0x00, 0x00, 0x43, 0x05, 0x41, 0x41};
|
|
|
|
static wmm_cof_record_t wmm_cof_entries[COEFFICIENTS_COUNT];
|
|
|
|
static float convert_varint_to_float(char **bytes);
|
|
|
|
float wmm_get_date(uint8_t year, uint8_t month, uint8_t date)
|
|
{
|
|
return (float)year + 2000.0f + (float)(month - 1U) / 12.0f + (float)(date - 1U) / (365.0f);
|
|
}
|
|
|
|
static float convert_varint_to_float(char **bytes)
|
|
{
|
|
float result;
|
|
int32_t result_int;
|
|
bool negative = false;
|
|
bool first_byte = true;
|
|
uint8_t shift;
|
|
|
|
do
|
|
{
|
|
if (first_byte)
|
|
{
|
|
if (**bytes & 0x40)
|
|
{
|
|
negative = true;
|
|
}
|
|
|
|
result_int = **bytes & 0x3f;
|
|
shift = 6U;
|
|
first_byte = false;
|
|
}
|
|
else
|
|
{
|
|
result_int += (uint32_t)(**bytes & 0x7f) << shift;
|
|
shift += 7U;
|
|
}
|
|
|
|
if ((**bytes & 0x80) == 0U)
|
|
{
|
|
(*bytes)++;
|
|
break;
|
|
}
|
|
|
|
(*bytes)++;
|
|
|
|
} while (true);
|
|
|
|
|
|
result = ((float)result_int) / 10.0f;
|
|
if (negative)
|
|
{
|
|
result = -result;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void wmm_init(void)
|
|
{
|
|
uint8_t j;
|
|
uint8_t m;
|
|
uint8_t n;
|
|
uint8_t D2;
|
|
float gnm;
|
|
float hnm;
|
|
float dgnm;
|
|
float dhnm;
|
|
float flnmj;
|
|
uint8_t i;
|
|
char *bytes = (char *)&wmm_cof_entries_encoded[0];
|
|
|
|
// unpack coefficients
|
|
for (i = 0U; i < COEFFICIENTS_COUNT; i++)
|
|
{
|
|
wmm_cof_entries[i].gnm = convert_varint_to_float(&bytes);
|
|
wmm_cof_entries[i].hnm = convert_varint_to_float(&bytes);
|
|
wmm_cof_entries[i].dgnm = convert_varint_to_float(&bytes);
|
|
wmm_cof_entries[i].dhnm = convert_varint_to_float(&bytes);
|
|
}
|
|
|
|
c[0][0] = 0.0f;
|
|
cd[0][0] = 0.0f;
|
|
|
|
j = 0U;
|
|
for (n = 1U; n <= 12U; n++)
|
|
{
|
|
for (m = 0U; m <= n; m++)
|
|
{
|
|
gnm = wmm_cof_entries[j].gnm;
|
|
hnm = wmm_cof_entries[j].hnm;
|
|
dgnm = wmm_cof_entries[j].dgnm;
|
|
dhnm = wmm_cof_entries[j].dhnm;
|
|
j++;
|
|
|
|
if (m <= n)
|
|
{
|
|
c[m][n] = gnm;
|
|
cd[m][n] = dgnm;
|
|
if (m != 0U)
|
|
{
|
|
c[n][m - 1U] = hnm;
|
|
cd[n][m - 1U] = dhnm;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// CONVERT SCHMIDT NORMALIZED GAUSS COEFFICIENTS TO UNNORMALIZED
|
|
*snorm = 1.0f;
|
|
for (n = 1U; n <= 12U; n++)
|
|
{
|
|
*(snorm + n) = *(snorm + n - 1U) * (float)(2U * n - 1U) / (float)n;
|
|
j = 2U;
|
|
m = 0U;
|
|
for (D2 = n - m + 1U; D2 > 0U; D2--)
|
|
{
|
|
k[m][n] = (float)(((n - 1U) * (n - 1U)) - (m * m)) / (float)((2U * n - 1U) * (2U * n - 3U));
|
|
if (m > 0U)
|
|
{
|
|
flnmj = (float)((n - m + 1U) * j) / (float)(n + m);
|
|
*(snorm + n + m * 13U) = *(snorm + n + (m - 1U) * 13U) * sqrt(flnmj);
|
|
j = 1U;
|
|
c[n][m - 1U] = *(snorm + n + m * 13U) * c[n][m - 1U];
|
|
cd[n][m - 1U] = *(snorm + n + m * 13U) * cd[n][m - 1U];
|
|
}
|
|
c[m][n] = *(snorm + n + m * 13U) * c[m][n];
|
|
cd[m][n] = *(snorm + n + m *13U) * cd[m][n];
|
|
m += 1U;
|
|
}
|
|
fn[n] = (float)(n + 1U);
|
|
fm[n] = (float)n;
|
|
}
|
|
k[1][1] = 0.0f;
|
|
}
|
|
|
|
void E0000(float glat, float glon, float time_years, float *dec)
|
|
{
|
|
static float tc[13][13];
|
|
static float sp[13];
|
|
static float cp[13];
|
|
static float dp[13][13];
|
|
static float pp[13];
|
|
float dt = time_years - WMM_EPOCH;
|
|
float rlon = glon * DEGREES_TO_RADIANS;
|
|
float rlat = glat * DEGREES_TO_RADIANS;
|
|
float srlon = sinf(rlon);
|
|
float srlat = sinf(rlat);
|
|
float crlon = cosf(rlon);
|
|
float crlat = cosf(rlat);
|
|
float srlat2 = srlat * srlat;
|
|
float crlat2 = crlat * crlat;
|
|
sp[0] = 0.0f;
|
|
sp[1] = srlon;
|
|
cp[0] = 1.0f;
|
|
cp[1] = crlon;
|
|
dp[0][0] = 0.0f;
|
|
pp[0] = 1.0f;
|
|
|
|
// CONVERT FROM GEODETIC COORDS. TO SPHERICAL COORDS
|
|
float q = sqrtf(A2_CONST - C2_CONST * srlat2);
|
|
float q2 = (A2_CONST / (B2_CONST)) * (A2_CONST / B2_CONST);
|
|
float ct = srlat / sqrtf(q2 * crlat2 + srlat2);
|
|
float st = sqrtf(1.0f - (ct * ct));
|
|
float r2 = (A4_CONST - C4_CONST * srlat2) / (q * q);
|
|
float r = sqrtf(r2);
|
|
float d = sqrtf(A2_CONST * crlat2 + B2_CONST * srlat2);
|
|
float ca = d / r;
|
|
float sa = C2_CONST * crlat * srlat / (r * d);
|
|
for (uint8_t m = 2U; m <= 12U; m++)
|
|
{
|
|
sp[m] = sp[1] * cp[m - 1U] + cp[1] * sp[m - 1U];
|
|
cp[m] = cp[1] * cp[m - 1U] - sp[1] * sp[m - 1U];
|
|
}
|
|
float aor = RE_CONST / r;
|
|
float ar = aor * aor;
|
|
float br = 0.0f;
|
|
float bt = 0.0f;
|
|
float bp = 0.0f;
|
|
float bpp = 0.0f;
|
|
|
|
for (uint16_t n = 1U; n <= 12U; n++)
|
|
{
|
|
ar = ar * aor;
|
|
uint8_t m = 0U;
|
|
for (uint8_t D4 = n + 1U; D4 > 0U; D4--)
|
|
{
|
|
// COMPUTE UNNORMALIZED ASSOCIATED LEGENDRE POLYNOMIALS AND DERIVATIVES VIA RECURSION RELATIONS
|
|
if (n == m)
|
|
{
|
|
*(snorm + n + m * 13U) = st * *(snorm + n - 1U + (m - 1U) * 13U);
|
|
dp[m][n] = st * dp[m - 1U][n - 1U] + ct * *(snorm + n - 1U + (m - 1U) * 13U);
|
|
goto S50;
|
|
}
|
|
if (n == 1U && m == 0U)
|
|
{
|
|
*(snorm + n + m * 13U) = ct * *(snorm + n - 1U + m * 13U);
|
|
dp[m][n] = ct * dp[m][n - 1U] - st * *(snorm + n - 1U + m * 13U);
|
|
goto S50;
|
|
}
|
|
if (n > 1U && n != m)
|
|
{
|
|
if (m > n - 2U)
|
|
{
|
|
*(snorm + n - 2U + m * 13U) = 0.0f;
|
|
}
|
|
if (m > n - 2U)
|
|
{
|
|
dp[m][n - 2U] = 0.0f;
|
|
}
|
|
*(snorm + n + m * 13U) = ct * *(snorm + n - 1U + m * 13U) - k[m][n] * *(snorm + n - 2U + m * 13U);
|
|
dp[m][n] = ct * dp[m][n - 1U] - st * *(snorm + n - 1U + m * 13U) - k[m][n] * dp[m][n - 2U];
|
|
}
|
|
S50:
|
|
|
|
// TIME ADJUST THE GAUSS COEFFICIENTS
|
|
tc[m][n] = c[m][n] + dt * cd[m][n];
|
|
if (m != 0U)
|
|
{
|
|
tc[n][m - 1U] = c[n][m - 1U] + dt * cd[n][m - 1U];
|
|
}
|
|
|
|
// ACCUMULATE TERMS OF THE SPHERICAL HARMONIC EXPANSIONS
|
|
float par = ar * *(snorm + n + m * 13U);
|
|
float temp1;
|
|
float temp2;
|
|
|
|
if (m == 0)
|
|
{
|
|
temp1 = tc[m][n] * cp[m];
|
|
temp2 = tc[m][n] * sp[m];
|
|
}
|
|
else
|
|
{
|
|
temp1 = tc[m][n] * cp[m] + tc[n][m - 1U] * sp[m];
|
|
temp2 = tc[m][n] * sp[m] - tc[n][m - 1U] * cp[m];
|
|
}
|
|
|
|
bt = bt - ar * temp1 * dp[m][n];
|
|
bp += (fm[m] * temp2 * par);
|
|
br += (fn[n] * temp1 * par);
|
|
|
|
// SPECIAL CASE: NORTH/SOUTH GEOGRAPHIC POLES
|
|
if (st == 0.0f && m == 1U)
|
|
{
|
|
if (n == 1U)
|
|
{
|
|
pp[n] = pp[n - 1U];
|
|
}
|
|
else
|
|
{
|
|
pp[n] = ct * pp[n - 1U] - k[m][n] * pp[n - 2U];
|
|
}
|
|
bpp += (fm[m] * temp2 * ar * pp[n]);
|
|
}
|
|
m += 1U;
|
|
}
|
|
}
|
|
if (st == 0.0f)
|
|
{
|
|
bp = bpp;
|
|
}
|
|
else
|
|
{
|
|
bp /= st;
|
|
}
|
|
|
|
// ROTATE MAGNETIC VECTOR COMPONENTS FROM SPHERICAL TO GEODETIC COORDINATES
|
|
float bx = -bt * ca - br * sa;
|
|
float by = bp;
|
|
|
|
// COMPUTE DECLINATION
|
|
*dec = atan2f(by, bx) / DEGREES_TO_RADIANS;
|
|
}
|