Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_geodetic_wmm2010.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>.
4  *
5  * This module based on the WMM2010 modell (http://www.ngdc.noaa.gov/geomag/models.shtml).
6  *
7  */
8 
9 #include "std.h"
11 
12 const double gh1[MAXCOEFF] = {
13 0.0,-29496.6,-1586.3,4944.4,
14 -2396.6,3026.1,-2707.7, 1668.6,-576.1,
15 1340.1,-2326.2,-160.2, 1231.9,251.9, 634,-536.6,
16 912.6,808.9,286.4,166.7,-211.2,-357.1,164.3,89.4,-309.1,
17 -230.9,357.2,44.6,200.3,188.9,-141.1,-118.2,-163.0, 0.0,-7.8,100.9,
18 72.8,68.6,-20.8,76.0,44.1,-141.4,61.5,-22.8,-66.3,13.2,3.1,-77.9,55.0,
19 80.5,-75.1,-57.9,-4.7,-21.1,45.3,6.5,13.9,24.9,10.4,7.0,1.7,-27.7,4.9,-3.3,
20 24.4,8.1,11.0,-14.5,-20.0,-5.6,11.9,-19.3,-17.4,11.5,16.7,10.9,7.0,-14.1,-10.8,-3.7,1.7,
21 5.4,9.4,-20.5,3.4,11.5,-5.2,12.8,3.1,-7.2,-12.4,-7.4,-0.7,8.0,8.4,2.1,-8.5,-6.1,-10.1,7.0,
22 -2.0,-6.3,2.8,0.9,-0.1,-1.1,4.7,-0.2,4.4,2.5,-7.2,-0.3,-1.0,2.2,-3.9,3.1,-2.0,-1.0,-2.0,-2.8,-8.3,
23 3.0,-1.5,0.2,-2.1,1.7,1.7,-0.6,-0.5,-1.8,0.5,0.9,-0.8,-0.4,0.4,-2.5,1.8,-1.3,0.1,-2.1,0.7,-1.9,3.8,-1.8,
24 -2.2,-0.2,-0.9,0.3,0.3,1.0,2.1,-0.6,-2.5,0.9,0.5,-0.1,0.6,0.5,0.0,-0.4,0.1,-0.4,0.3,0.2,-0.9,-0.8,-0.2,0.0,0.9
25 };
26 
27 const double gh2[MAXCOEFF] = {
28 0.0,11.6,16.5,-25.9,
29 -12.1,-4.4,-22.5,1.9,-11.8,
30 0.4,-4.1,7.3,-2.9,-3.9,-7.7,-2.6,
31 -1.8,2.3,1.1,-8.7,2.7,4.6,3.9,-2.1,-0.8,
32 -1.0,0.6,0.4,-1.8,1.8,-1.0,1.2,0.9,4.0,1.0,-0.6,
33 -0.2,-0.2,-0.2,-0.1,-2.1,2.0,-0.4,-1.7,-0.6,-0.3,0.5,1.7,0.9,
34 0.1,-0.1,0.7,-0.6,0.3,1.3,-0.1,0.4,-0.1,0.3,-0.8,-0.7,-0.3,0.6,0.3,
35 -0.1,0.1,-0.1,-0.6,0.2,0.2,0.4,-0.2,0.4,0.3,0.1,0.3,-0.1,-0.6,0.4,0.2,0.3,
36 0.0,-0.1,0.0,0.0,-0.2,0.3,0.0,-0.4,-0.1,-0.3,0.1,0.1,0.0,-0.1,-0.2,-0.4,0.3,-0.2,0.2,
37 0.0,0.0,0.1,-0.1,-0.1,0.2,0.0,0.0,-0.1,-0.1,-0.1,-0.2,0.0,0.0,-0.1,-0.1,-0.2,-0.2,0.0,-0.2,-0.1,
38 0.0,0.0,0.0,0.0,0.1,0.1,0.0,0.0,0.1,0.0,0.0,0.0,0.1,0.0,0.0,0.0,-0.1,0.0,-0.1,-0.1,0.0,0.0,-0.1,
39 0.0,0.0,0.0,0.1,0.0,0.1,0.0,-0.1,0.0,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.1,0.0,0.1,0.0
40 };
41 
42 int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh) {
43 
44  int16_t nmax;
45  int16_t k, l;
46  int16_t ii;
47  double factor;
48 
49  factor = date - dte1;
50  if (nmax1 == nmax2) {
51  k = nmax1 * (nmax1 + 2);
52  nmax = nmax1;
53  }
54  else {
55  if (nmax1 > nmax2) {
56  k = nmax2 * (nmax2 + 2);
57  l = nmax1 * (nmax1 + 2);
58 
59  for ( ii = k + 1; ii <= l; ++ii) {
60  gh[ii] = gh1[ii];
61  }
62 
63  nmax = nmax1;
64  }
65  else {
66  k = nmax1 * (nmax1 + 2);
67  l = nmax2 * (nmax2 + 2);
68 
69  for ( ii = k + 1; ii <= l; ++ii) {
70  gh[ii] = factor * gh2[ii];
71  }
72 
73  nmax = nmax2;
74  }
75  }
76 
77  for ( ii = 1; ii <= k; ++ii) {
78  gh[ii] = gh1[ii] + factor * gh2[ii];
79  }
80 
81  return(nmax);
82 }
83 
84 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) {
85 
86  double earths_radius = 6371.2;
87  double dtr = 0.01745329;
88  double slat;
89  double clat;
90  double ratio;
91  double aa, bb, cc, dd;
92  double sd;
93  double cd;
94  double r;
95  double a2;
96  double b2;
97  double rr = 0;
98  double fm,fn = 0;
99  double sl[14];
100  double cl[14];
101 #ifdef GEO_MAG_DOUBLE
102  double p[119];
103  double q[119];
104 #else
105  float p[119];
106  float q[119];
107 #endif
108  int ii,j,k,l,m,n;
109  int npq;
110  int ios;
111  double argument;
112  double power;
113  a2 = 40680631.59; /* WGS84 */
114  b2 = 40408299.98; /* WGS84 */
115  ios = 0;
116  r = elev;
117  argument = flat * dtr;
118  slat = sinf( argument );
119  if ((90.0 - flat) < 0.001) {
120  aa = 89.999; /* 300 ft. from North pole */
121  }
122  else {
123  if ((90.0 + flat) < 0.001) {
124  aa = -89.999; /* 300 ft. from South pole */
125  }
126  else {
127  aa = flat;
128  }
129  }
130  argument = aa * dtr;
131  clat = cosf( argument );
132  argument = flon * dtr;
133  sl[1] = sinf( argument );
134  cl[1] = cosf( argument );
135 
136  *geo_mag_x = 0;
137  *geo_mag_y = 0;
138  *geo_mag_z = 0;
139 
140  sd = 0.0;
141  cd = 1.0;
142  l = 1;
143  n = 0;
144  m = 1;
145  npq = (nmax * (nmax + 3)) / 2;
146 
147  if (igdgc == 1) {
148  aa = a2 * clat * clat;
149  bb = b2 * slat * slat;
150  cc = aa + bb;
151  argument = cc;
152  dd = sqrt( argument );
153  argument = elev * (elev + 2.0 * dd) + (a2 * aa + b2 * bb) / cc;
154  r = sqrt( argument );
155  cd = (elev + dd) / r;
156  sd = (a2 - b2) / dd * slat * clat / r;
157  aa = slat;
158  slat = slat * cd - clat * sd;
159  clat = clat * cd + aa * sd;
160  }
161 
162  ratio = earths_radius / r;
163  argument = 3.0;
164  aa = sqrt( argument );
165  p[1] = 2.0 * slat;
166  p[2] = 2.0 * clat;
167  p[3] = 4.5 * slat * slat - 1.5;
168  p[4] = 3.0 * aa * clat * slat;
169  q[1] = -clat;
170  q[2] = slat;
171  q[3] = -3.0 * clat * slat;
172  q[4] = aa * (slat * slat - clat * clat);
173 
174  for ( k = 1; k <= npq; ++k) {
175  if (n < m) {
176  m = 0;
177  n = n + 1;
178  argument = ratio;
179  power = n + 2;
180  rr = pow(argument,power);
181  fn = n;
182  }
183  fm = m;
184  if (k >= 5) {
185  if (m == n) {
186  argument = (1.0 - 0.5/fm);
187  aa = sqrt( argument );
188  j = k - n - 1;
189  p[k] = (1.0 + 1.0/fm) * aa * clat * p[j];
190  q[k] = aa * (clat * q[j] + slat/fm * p[j]);
191  sl[m] = sl[m-1] * cl[1] + cl[m-1] * sl[1];
192  cl[m] = cl[m-1] * cl[1] - sl[m-1] * sl[1];
193  }
194  else {
195  argument = fn*fn - fm*fm;
196  aa = sqrt( argument );
197  argument = ((fn - 1.0)*(fn-1.0)) - (fm * fm);
198  bb = sqrt( argument )/aa;
199  cc = (2.0 * fn - 1.0)/aa;
200  ii = k - n;
201  j = k - 2 * n + 1;
202  p[k] = (fn + 1.0) * (cc * slat/fn * p[ii] - bb/(fn - 1.0) * p[j]);
203  q[k] = cc * (slat * q[ii] - clat/fn * p[ii]) - bb * q[j];
204  }
205  }
206 
207  aa = rr * gh[l];
208 
209  if (m == 0) {
210  *geo_mag_x = *geo_mag_x + aa * q[k];
211  *geo_mag_z = *geo_mag_z - aa * p[k];
212  l = l + 1;
213  }
214  else {
215  bb = rr * gh[l+1];
216  cc = aa * cl[m] + bb * sl[m];
217  *geo_mag_x = *geo_mag_x + cc * q[k];
218  *geo_mag_z = *geo_mag_z - cc * p[k];
219  if (clat > 0) {
220  *geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) *
221  fm * p[k]/((fn + 1.0) * clat);
222  }
223  else {
224  *geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) * q[k] * slat;
225  }
226  l = l + 2;
227  }
228  m = m + 1;
229  }
230  if (iext != 0) {
231  aa = ext2 * cl[1] + ext3 * sl[1];
232  *geo_mag_x = *geo_mag_x - ext1 * clat + aa * slat;
233  *geo_mag_y = *geo_mag_y + ext2 * sl[1] - ext3 * cl[1];
234  *geo_mag_z = *geo_mag_z + ext1 * slat + aa * clat;
235  }
236  aa = *geo_mag_x;
237  *geo_mag_x = *geo_mag_x * cd + *geo_mag_z * sd;
238  *geo_mag_z = *geo_mag_z * cd - aa * sd;
239  return(ios);
240 }
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)
#define MAXCOEFF
const double gh1[MAXCOEFF]
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
signed short int16_t
Definition: types.h:17
static float p[2][2]
const double gh2[MAXCOEFF]
int32_t m[3 *3]