void geo_mag_event(void)
{
if (geo_mag_calc_flag) {
double gha[MAXCOEFF]; // Geomag global variables
int32_t nmax;
/* Current date in decimal year, for example 2012.68 */
double sdate = GPS_EPOCH_BEGIN +
(double)gps.week / WEEKS_IN_YEAR +
(double)gps.tow / 1000 / SECS_IN_YEAR;
/* LLA Position in decimal degrees and altitude in km */
double latitude = (double)gps.lla_pos.lat / 1e7;
double longitude = (double)gps.lla_pos.lon / 1e7;
double alt = (double)gps.lla_pos.alt / 1e6;
// Calculates additional coeffs
nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
// Calculates absolute magnet fields
mag_calc(1, latitude, longitude, alt, nmax, gha,
&geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
double_vect3_normalize(&geo_mag.vect);
// copy to ahrs
#ifdef AHRS_FLOAT
VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
geo_mag.ready = TRUE;
}
geo_mag_calc_flag = FALSE;
}
void geomag(geomag_vector *gStr) {
/* Variable declaration */
double sdate = gStr->sdate;
double latitude = gStr->latitude;
double longitude = gStr->longitude;
double alt = gStr->alt;
/* Control variables */
uint8_t igdgc = 2; /* GEOCENTRIC */
static int max1;
static int max2;
static int max3;
static int nmax;
static uint8_t model[] = "IGRF2015";
static double epoch;
static double yrmin;
static double yrmax;
static double minyr;
static double maxyr;
static double altmin;
static double altmax;
static double minalt;
static double maxalt;
/* Obtain the desired model file and read the data */
if (PREV_SDATE == 0) {
epoch = 2015.00;
max1 = 13;
max2 = 8;
max3 = 0;
yrmin = 2015.00;
yrmax = 2020.00;
altmin = -1.0;
altmax = 600.0;
minyr = yrmin;
maxyr = yrmax;
}
PREV_SDATE = sdate;
/* Get altitude min and max for selected model. */
minalt = altmin;
maxalt = altmax;
/* Get Coordinate prefs */
/* If needed modify ranges to reflect coords. */
if (igdgc == 2) {
minalt += 6371.2; /* Add radius to ranges. */
maxalt += 6371.2;
}
/** This will compute everything needed for 1 point in time. **/
getshc(1, max1, 1);
getshc(0, max2, 2);
nmax = extrapsh(sdate, epoch, max1, max2, 3);
nmax = extrapsh(sdate + 1, epoch, max1, max2, 4);
/* Do the first calculations */
shval3(igdgc, latitude, longitude, alt, nmax, 3, IEXT, EXT_COEFF1,
EXT_COEFF2,
EXT_COEFF3);
dihf(3);
shval3(igdgc, latitude, longitude, alt, nmax, 4, IEXT, EXT_COEFF1,
EXT_COEFF2,
EXT_COEFF3);
dihf(4);
d = d * (RAD2DEG);
i = i * (RAD2DEG);
/* deal with geographic and magnetic poles */
/* at magnetic poles */
if (h < 100.0) {
d = GEO_NAN;
/* while rest is ok */
}
/* at geographic poles */
if (90.0 - fabs(latitude) <= 0.001) {
x = GEO_NAN;
y = GEO_NAN;
d = GEO_NAN;
/* while rest is ok */
}
/** Above will compute everything for 1 point in time. **/
gStr->Xm = x;
gStr->Ym = y;
gStr->Zm = z;
gStr->decl = d;
gStr->incl = i;
gStr->h = h;
gStr->f = f;
}
