Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_geodetic_wmm2020.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
3  * Copyright (C) 2020 OpenUAS info@openuas.org>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 #include "std.h"
30 #include "math/pprz_geodetic_wmm2020.h"
31 
32 const double gh1[MAXCOEFF] = {
33  //WMM 2020 data
34  0.0, -29404.5, -1450.7, 4652.9,
35  -2500.0, 2982.0, -2991.6, 1676.8, -734.8,
36  1363.9, -2381.0, -82.2, 1236.2, 241.8, 525.7, -542.9,
37  903.1, 809.4, 282.0, 86.2, -158.4, -309.4, 199.8, 47.9, -350.1,
38  -234.4, 363.1, 47.7, 187.8, 208.4, -140.7, -121.3, -151.2, 32.2, 13.7, 99.1,
39  65.9, 65.6, -19.1, 73.0, 25.0, -121.5, 52.7, -36.2, -64.4, 13.5, 9.0, -64.7, 68.1,
40  80.6, -76.8, -51.4, -8.3, -16.8, 56.5, 2.3, 15.8, 23.5, 6.4, -2.2, -7.2, -27.2, 9.8, -1.9,
41  23.6, 9.8, 8.4, -17.5, -15.3, -0.4, 12.8, -21.1, -11.8, 15.3, 14.9, 13.7, 3.6, -16.5, -6.9, -0.3, 2.8,
42  5.0, 8.2, -23.3, 2.9, 11.1, -1.4, 9.8, -1.1, -5.1, -13.3, -6.2, 1.1, 7.8, 8.9, 0.4, -9.3, -1.5, -11.9, 9.7,
43  -1.9, -6.2, 3.4, -0.1, -0.2, 1.7, 3.5, -0.9, 4.8, 0.6, -8.6, -0.9, -0.1, 1.9, -4.2, 1.4, -3.4, -2.4, -0.1, -3.9, -8.8,
44  3.0, -1.4, 0.0, -2.5, 2.6, 2.4, -0.5, -0.9, -0.4, 0.3, 0.6, -0.7, -0.2, -0.1, -1.7, 1.4, -1.6, -0.6, -3.0, 0.2, -2.0, 3.1, -2.6,
45  -2.0, -0.1, -1.2, 0.5, 0.5, 1.3, 1.3, -1.2, -1.8, 0.7, 0.1, 0.3, 0.7, 0.5, -0.1, -0.2, 0.6, -0.5, 0.2, 0.1, -0.9, -1.1, 0.0, -0.3, 0.5
46 };
47 
48 const double gh2[MAXCOEFF] = {
49  //WMM 2020 data
50  0.0, 6.7, 7.7, -25.1,
51  -11.5, -7.1, -30.2, -2.2, -23.9,
52  2.8, -6.2, 5.7, 3.4, -1.0, -12.2, 1.1,
53  -1.1, -1.6, 0.2, -6.0, 6.9, 5.4, 3.7, -5.5, -5.6,
54  -0.3, 0.6, 0.1, -0.7, 2.5, 0.1, -0.9, 1.2, 3.0, 1.0, 0.5,
55  -0.6, -0.4, 0.1, 0.5, -1.8, 1.4, -1.4, -1.4, 0.9, 0.0, 0.1, 0.8, 1.0,
56  -0.1, -0.3, 0.5, -0.1, 0.6, 0.7, -0.7, 0.2, -0.2, -0.5, -1.2, -0.8, 0.2, 1.0, 0.3,
57  -0.1, 0.1, -0.3, -0.1, 0.7, 0.5, -0.2, -0.1, 0.5, 0.4, -0.3, 0.5, -0.5, 0.0, 0.4, 0.4, 0.1,
58  -0.1, -0.2, -0.3, 0.0, 0.2, 0.4, -0.4, -0.3, 0.4, 0.0, 0.1, 0.3, 0.0, 0.0, -0.2, 0.0, 0.5, -0.4, 0.2,
59  0.0, 0.0, 0.0, 0.0, 0.1, 0.2, -0.3, -0.1, 0.1, -0.2, -0.2, 0.0, 0.1, -0.1, 0.0, -0.2, -0.1, -0.1, 0.2, 0.0, 0.0,
60  0.0, -0.1, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.2, -0.1, 0.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, -0.1, -0.1, -0.1, 0.0, -0.1, 0.0,
61  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, -0.1
62 };
63 
64 int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
65 {
66 
67  int16_t nmax;
68  int16_t k, l;
69  int16_t ii;
70  double factor;
71 
72  factor = date - dte1;
73  if (nmax1 == nmax2) {
74  k = nmax1 * (nmax1 + 2);
75  nmax = nmax1;
76  } else {
77  if (nmax1 > nmax2) {
78  k = nmax2 * (nmax2 + 2);
79  l = nmax1 * (nmax1 + 2);
80 
81  for (ii = k + 1; ii <= l; ++ii) {
82  gh[ii] = gh1[ii];
83  }
84 
85  nmax = nmax1;
86  } else {
87  k = nmax1 * (nmax1 + 2);
88  l = nmax2 * (nmax2 + 2);
89 
90  for (ii = k + 1; ii <= l; ++ii) {
91  gh[ii] = factor * gh2[ii];
92  }
93 
94  nmax = nmax2;
95  }
96  }
97 
98  for (ii = 1; ii <= k; ++ii) {
99  gh[ii] = gh1[ii] + factor * gh2[ii];
100  }
101 
102  return (nmax);
103 }
104 
105 int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double *gh, double *geo_mag_x,
106  double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3)
107 {
108 
109  double earths_radius = 6371.2;
110  double dtr = 0.01745329;
111  double slat;
112  double clat;
113  double ratio;
114  double aa, bb, cc, dd;
115  double sd;
116  double cd;
117  double r;
118  double a2;
119  double b2;
120  double rr = 0;
121  double fm, fn = 0;
122  double sl[14];
123  double cl[14];
124 #ifdef GEO_MAG_DOUBLE
125  double p[119];
126  double q[119];
127 #else
128  float p[119];
129  float q[119];
130 #endif
131  int ii, j, k, l, m, n;
132  int npq;
133  int ios;
134  double argument;
135  double power;
136  a2 = 40680631.59; /* WGS84 */
137  b2 = 40408299.98; /* WGS84 */
138  ios = 0;
139  r = elev;
140  argument = flat * dtr;
141  slat = sinf(argument);
142  if ((90.0 - flat) < 0.001) {
143  aa = 89.999; /* in meters from North pole */
144  } else {
145  if ((90.0 + flat) < 0.001) {
146  aa = -89.999; /* in meters from South pole */
147  } else {
148  aa = flat;
149  }
150  }
151  argument = aa * dtr;
152  clat = cosf(argument);
153  argument = flon * dtr;
154  sl[1] = sinf(argument);
155  cl[1] = cosf(argument);
156 
157  *geo_mag_x = 0;
158  *geo_mag_y = 0;
159  *geo_mag_z = 0;
160 
161  sd = 0.0;
162  cd = 1.0;
163  l = 1;
164  n = 0;
165  m = 1;
166  npq = (nmax * (nmax + 3)) / 2;
167 
168  if (igdgc == 1) {
169  aa = a2 * clat * clat;
170  bb = b2 * slat * slat;
171  cc = aa + bb;
172  argument = cc;
173  dd = sqrt(argument);
174  argument = elev * (elev + 2.0 * dd) + (a2 * aa + b2 * bb) / cc;
175  r = sqrt(argument);
176  cd = (elev + dd) / r;
177  sd = (a2 - b2) / dd * slat * clat / r;
178  aa = slat;
179  slat = slat * cd - clat * sd;
180  clat = clat * cd + aa * sd;
181  }
182 
183  ratio = earths_radius / r;
184  argument = 3.0;
185  aa = sqrt(argument);
186  p[1] = 2.0 * slat;
187  p[2] = 2.0 * clat;
188  p[3] = 4.5 * slat * slat - 1.5;
189  p[4] = 3.0 * aa * clat * slat;
190  q[1] = -clat;
191  q[2] = slat;
192  q[3] = -3.0 * clat * slat;
193  q[4] = aa * (slat * slat - clat * clat);
194 
195  for (k = 1; k <= npq; ++k) {
196  if (n < m) {
197  m = 0;
198  n = n + 1;
199  argument = ratio;
200  power = n + 2;
201  rr = pow(argument, power);
202  fn = n;
203  }
204  fm = m;
205  if (k >= 5) {
206  if (m == n) {
207  argument = (1.0 - 0.5 / fm);
208  aa = sqrt(argument);
209  j = k - n - 1;
210  p[k] = (1.0 + 1.0 / fm) * aa * clat * p[j];
211  q[k] = aa * (clat * q[j] + slat / fm * p[j]);
212  sl[m] = sl[m - 1] * cl[1] + cl[m - 1] * sl[1];
213  cl[m] = cl[m - 1] * cl[1] - sl[m - 1] * sl[1];
214  } else {
215  argument = fn * fn - fm * fm;
216  aa = sqrt(argument);
217  argument = ((fn - 1.0) * (fn - 1.0)) - (fm * fm);
218  bb = sqrt(argument) / aa;
219  cc = (2.0 * fn - 1.0) / aa;
220  ii = k - n;
221  j = k - 2 * n + 1;
222  p[k] = (fn + 1.0) * (cc * slat / fn * p[ii] - bb / (fn - 1.0) * p[j]);
223  q[k] = cc * (slat * q[ii] - clat / fn * p[ii]) - bb * q[j];
224  }
225  }
226 
227  aa = rr * gh[l];
228 
229  if (m == 0) {
230  *geo_mag_x = *geo_mag_x + aa * q[k];
231  *geo_mag_z = *geo_mag_z - aa * p[k];
232  l = l + 1;
233  } else {
234  bb = rr * gh[l + 1];
235  cc = aa * cl[m] + bb * sl[m];
236  *geo_mag_x = *geo_mag_x + cc * q[k];
237  *geo_mag_z = *geo_mag_z - cc * p[k];
238  if (clat > 0) {
239  *geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) *
240  fm * p[k] / ((fn + 1.0) * clat);
241  } else {
242  *geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) * q[k] * slat;
243  }
244  l = l + 2;
245  }
246  m = m + 1;
247  }
248  if (iext != 0) {
249  aa = ext2 * cl[1] + ext3 * sl[1];
250  *geo_mag_x = *geo_mag_x - ext1 * clat + aa * slat;
251  *geo_mag_y = *geo_mag_y + ext2 * sl[1] - ext3 * cl[1];
252  *geo_mag_z = *geo_mag_z + ext1 * slat + aa * clat;
253  }
254  aa = *geo_mag_x;
255  *geo_mag_x = *geo_mag_x * cd + *geo_mag_z * sd;
256  *geo_mag_z = *geo_mag_z * cd - aa * sd;
257  return (ios);
258 }
pprz_geodetic_wmm2020.h
WMM2020 Geomagnetic field model.
MAXCOEFF
#define MAXCOEFF
Definition: pprz_geodetic_wmm2020.h:67
extrapsh
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
Definition: pprz_geodetic_wmm2020.c:64
std.h
int16_t
signed short int16_t
Definition: types.h:17
Int32RMat::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:160
gh1
const double gh1[MAXCOEFF]
Definition: pprz_geodetic_wmm2020.c:32
gh2
const double gh2[MAXCOEFF]
Definition: pprz_geodetic_wmm2020.c:48
mag_calc
int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double *gh, double *geo_mag_x, double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3)
Definition: pprz_geodetic_wmm2020.c:105
logger_uart_parse.a2
a2
Definition: logger_uart_parse.py:9
p
static float p[2][2]
Definition: ins_alt_float.c:268