Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pprz_geodetic_wgs84.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Christophe De Wagter
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
43 #ifndef PPRZ_GEODETIC_WGS84_H
44 #define PPRZ_GEODETIC_WGS84_H
45 
46 #ifdef __cplusplus
47 extern "C" {
48 #endif
49 
50 #include "std.h"
51 
52 static const int8_t pprz_geodetic_wgs84_int[19][36] = {
53  {13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13},
54  {3, 1, -2, -3, -3, -3, -1, 3, 1, 5, 9, 11, 19, 27, 31, 34, 33, 34, 33, 34, 28, 23, 17, 13, 9, 4, 4, 1, -2, -2, 0, 2, 3, 2, 1, 1},
55  {2, 2, 1, -1, -3, -7, -14, -24, -27, -25, -19, 3, 24, 37, 47, 60, 61, 58, 51, 43, 29, 20, 12, 5, -2, -10, -14, -12, -10, -14, -12, -6, -2, 3, 6, 4},
56  {2, 9, 17, 10, 13, 1, -14, -30, -39, -46, -42, -21, 6, 29, 49, 65, 60, 57, 47, 41, 21, 18, 14, 7, -3, -22, -29, -32, -32, -26, -15, -2, 13, 17, 19, 6},
57  { -8, 8, 8, 1, -11, -19, -16, -18, -22, -35, -40, -26, -12, 24, 45, 63, 62, 59, 47, 48, 42, 28, 12, -10, -19, -33, -43, -42, -43, -29, -2, 17, 23, 22, 6, 2},
58  { -12, -10, -13, -20, -31, -34, -21, -16, -26, -34, -33, -35, -26, 2, 33, 59, 52, 51, 52, 48, 35, 40, 33, -9, -28, -39, -48, -59, -50, -28, 3, 23, 37, 18, -1, -11},
59  { -7, -5, -8, -15, -28, -40, -42, -29, -22, -26, -32, -51, -40, -17, 17, 31, 34, 44, 36, 28, 29, 17, 12, -20, -15, -40, -33, -34, -34, -28, 7, 29, 43, 20, 4, -6},
60  {5, 10, 7, -7, -23, -39, -47, -34, -9, -10, -20, -45, -48, -32, -9, 17, 25, 31, 31, 26, 15, 6, 1, -29, -44, -61, -67, -59, -36, -11, 21, 39, 49, 39, 22, 10},
61  {13, 12, 11, 2, -11, -28, -38, -29, -10, 3, 1, -11, -41, -42, -16, 3, 17, 33, 22, 23, 2, -3, -7, -36, -59, -90, -95, -63, -24, 12, 53, 60, 58, 46, 36, 26},
62  {22, 16, 17, 13, 1, -12, -23, -20, -14, -3, 14, 10, -15, -27, -18, 3, 12, 20, 18, 12, -13, -9, -28, -49, -62, -89, -102, -63, -9, 33, 58, 73, 74, 63, 50, 32},
63  {36, 22, 11, 6, -1, -8, -10, -8, -11, -9, 1, 32, 4, -18, -13, -9, 4, 14, 12, 13, -2, -14, -25, -32, -38, -60, -75, -63, -26, 0, 35, 52, 68, 76, 64, 52},
64  {51, 27, 10, 0, -9, -11, -5, -2, -3, -1, 9, 35, 20, -5, -6, -5, 0, 13, 17, 23, 21, 8, -9, -10, -11, -20, -40, -47, -45, -25, 5, 23, 45, 58, 57, 63},
65  {46, 22, 5, -2, -8, -13, -10, -7, -4, 1, 9, 32, 16, 4, -8, 4, 12, 15, 22, 27, 34, 29, 14, 15, 15, 7, -9, -25, -37, -39, -23, -14, 15, 33, 34, 45},
66  {21, 6, 1, -7, -12, -12, -12, -10, -7, -1, 8, 23, 15, -2, -6, 6, 21, 24, 18, 26, 31, 33, 39, 41, 30, 24, 13, -2, -20, -32, -33, -27, -14, -2, 5, 20},
67  { -15, -18, -18, -16, -17, -15, -10, -10, -8, -2, 6, 14, 13, 3, 3, 10, 20, 27, 25, 26, 34, 39, 45, 45, 38, 39, 28, 13, -1, -15, -22, -22, -18, -15, -14, -10},
68  { -45, -43, -37, -32, -30, -26, -23, -22, -16, -10, -2, 10, 20, 20, 21, 24, 22, 17, 16, 19, 25, 30, 35, 35, 33, 30, 27, 10, -2, -14, -23, -30, -33, -29, -35, -43},
69  { -61, -60, -61, -55, -49, -44, -38, -31, -25, -16, -6, 1, 4, 5, 4, 2, 6, 12, 16, 16, 17, 21, 20, 26, 26, 22, 16, 10, -1, -16, -29, -36, -46, -55, -54, -59},
70  { -53, -54, -55, -52, -48, -42, -38, -38, -29, -26, -26, -24, -23, -21, -19, -16, -12, -8, -4, -1, 1, 4, 4, 6, 5, 4, 2, -6, -15, -24, -33, -40, -48, -50, -53, -52},
71  { -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30, -30}
72 };
73 
74 #define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)])
75 
82 {
83  float x = (180.0f + (float)lon / 1e7) / 10.0f;
84  Bound(x, 0.0f, 35.99999f);
85  float y = (90.0f - (float)lat / 1e7) / 10.0f;
86  Bound(y, 0.0f, 17.99999f);
87  uint8_t ex1 = (uint8_t) x;
88  uint8_t ex2 = ex1 + 1;
89  if (ex2 >= 36) { ex2 = 0; }
90  uint8_t ey1 = (uint8_t) y;
91  uint8_t ey2 = ey1 + 1;
92  float lin_x = x - ((float) ex1);
93  float lin_y = y - ((float) ey1);
94  float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1, ey1);
95  float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2, ey1);
96  float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1, ey2);
97  float h22 = lin_x * lin_y * WGS84_H(ex2, ey2);
98  return (uint32_t)((h11 + h12 + h21 + h22) * 1000.);
99 }
100 
106 static inline float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
107 {
108  float x = (180.0f + DegOfRad(lon)) / 10.0f;
109  Bound(x, 0.0f, 35.99999f);
110  float y = (90.0f - DegOfRad(lat)) / 10.0f;
111  Bound(y, 0.0f, 17.99999f);
112  uint8_t ex1 = (uint8_t) x;
113  uint8_t ex2 = ex1 + 1;
114  if (ex2 >= 36) { ex2 = 0; }
115  uint8_t ey1 = (uint8_t) y;
116  uint8_t ey2 = ey1 + 1;
117  float lin_x = x - ((float) ex1);
118  float lin_y = y - ((float) ey1);
119  float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1, ey1);
120  float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2, ey1);
121  float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1, ey2);
122  float h22 = lin_x * lin_y * WGS84_H(ex2, ey2);
123  return h11 + h12 + h21 + h22;
124 }
125 
126 #ifdef __cplusplus
127 } /* extern "C" */
128 #endif
129 
130 #endif /* PPRZ_GEODETIC_WGS84_H */
#define WGS84_H(x, y)
static const int8_t pprz_geodetic_wgs84_int[19][36]
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103