Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_geodetic_wmm2015.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
3  * Copyright (C) 2015 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"
31 
32 const double gh1[MAXCOEFF] = {
33  //WMM 2015 data
34  0.0, -29438.5, -1501.1, 4796.2,
35  -2445.3, 3012.5, -2845.6, 1676.6, -642.0,
36  1351.1, -2352.3, -115.3, 1225.6, 245.0, 581.9, -538.3,
37  907.2, 813.7, 283.4, 120.3, -188.6, -335.0, 180.9, 70.3, -329.5,
38  -232.6, 360.1, 47.4, 192.4, 196.9, -141.0, -119.4, -157.4, 16.1, 4.3, 100.1,
39  69.5, 67.4, -20.7, 72.8, 33.2, -129.8, 58.8, -29.0, -66.5, 13.2, 7.3, -70.9, 62.5,
40  81.6, -76.1, -54.1, -6.8, -19.4, 51.9, 5.6, 15.0, 24.4, 9.3, 3.3, -2.8, -27.5, 6.7, -2.3,
41  24.0, 8.6, 10.2, -16.9, -18.1, -3.2, 13.2, -20.6, -14.6, 13.3, 16.2, 11.7, 5.7, -16.0, -9.1, -2.0, 2.2,
42  5.4, 8.8, -21.6, 3.1, 10.8, -3.1, 11.7, 0.6, -6.8, -13.3, -6.9, -0.1, 7.8, 8.7, 1.0, -9.1, -3.9, -10.5, 8.5,
43  -1.9, -6.5, 3.3, 0.2, -0.3, 0.6, 4.6, -0.6, 4.4, 1.7, -7.9, -0.7, -0.6, 2.1, -4.1, 2.3, -2.8, -1.8, -1.1, -3.6, -8.7,
44  3.1, -1.5, -0.1, -2.3, 2.1, 2.1, -0.7, -0.9, -1.1, 0.6, 0.7, -0.7, -0.2, 0.2, -2.1, 1.7, -1.5, -0.2, -2.5, 0.4, -2.0, 3.5, -2.3,
45  -2.0, -0.3, -1.0, 0.4, 0.5, 1.3, 1.8, -0.9, -2.2, 0.9, 0.3, 0.1, 0.7, 0.5, -0.1, -0.4, 0.3, -0.4, 0.2, 0.2, -0.9, -0.9, -0.2, 0.0, 0.7
46 };
47 
48 const double gh2[MAXCOEFF] = {
49  //WMM 2015 data
50  0.0, 10.7, 17.9, -26.8,
51  -8.6, 0.0, -3.3, -27.1, 2.4, -13.3,
52  3.1, -6.2, 8.4, -0.4, -0.4, -10.4, 2.3,
53  -0.4, 0.8, -0.6, -9.2, 5.3, 4.0, 3.0, -4.2, -5.3,
54  -0.2, 0.1, 0.4, -1.4, 1.6, 0.0, -1.1, 1.3, 3.3, 3.8, 0.1,
55  -0.5, -0.2, 0.0, -0.6, -2.2, 2.4, -0.7, -1.1, 0.1, 0.3, 1.0, 1.5, 1.3,
56  0.2, -0.2, 0.7, -0.4, 0.5, 1.3, -0.2, 0.2, -0.1, -0.4, -0.7, -0.9, 0.1, 0.3, 0.1,
57  0.0, 0.1, -0.3, -0.5, 0.3, 0.5, 0.3, -0.2, 0.6, 0.4, -0.1, 0.2, -0.2, -0.4, 0.3, 0.3, 0.0,
58  0.0, -0.1, -0.2, -0.1, -0.1, 0.4, -0.2, -0.5, 0.1, -0.2, 0.1, 0.1, 0.0, 0.0, -0.2, -0.2, 0.4, -0.1, 0.3,
59  0.0, 0.0, 0.1, -0.1, -0.1, 0.3, 0.0, -0.1, 0.0, -0.1, -0.2, -0.1, 0.1, 0.0, -0.1, -0.2, -0.2, -0.1, 0.1, -0.2, -0.1,
60  0.0, 0.0, 0.0, -0.1, 0.1, 0.1, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, -0.1, -0.1, 0.0, -0.1, -0.1,
61  0.1, 0.0, 0.0, 0.0, 0.0, 0.1, -0.1, -0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
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 }
WMM2015 Geomagnetic field model.
#define MAXCOEFF
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
int32_t m[3 *3]
const double gh1[MAXCOEFF]
const double gh2[MAXCOEFF]
signed short int16_t
Definition: types.h:17
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)
static float p[2][2]