Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

[math] add wgs84_ellipsoid_to_geoid_i

Browse Source
This commit is contained in:
Felix Ruess
2016-02-07 15:48:53 +01:00
parent 2274b3eb30
commit a482053be9
1 changed files with 27 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_geodetic_wgs84.h
+27
View File
@@ -47,6 +47,8 @@
extern "C" {
#endif
#include "std.h"
const int8_t pprz_geodetic_wgs84_int[19][36] = {
{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},
{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},
@@ -71,6 +73,31 @@ const int8_t pprz_geodetic_wgs84_int[19][36] = {
#define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)])
/** Get WGS84 ellipsoid/geoid separation.
* @param[in] lat Latitude in 1e7deg
* @param[in] lon Longitude in 1e7deg
* @return geoid separation in m
*/
static inline float wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_i lon)
{
float x = (180.0f + (float)lon / 1e7) / 10.0f;
Bound(x, 0.0f, 35.99999f);
float y = (90.0f - (float)lat / 1e7) / 10.0f;
Bound(y, 0.0f, 17.99999f);
uint8_t ex1 = (uint8_t) x;
uint8_t ex2 = ex1 + 1;
if (ex2 >= 36) { ex2 = 0; }
uint8_t ey1 = (uint8_t) y;
uint8_t ey2 = ey1 + 1;
float lin_x = x - ((float) ex1);
float lin_y = y - ((float) ey1);
float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1, ey1);
float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2, ey1);
float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1, ey2);
float h22 = lin_x * lin_y * WGS84_H(ex2, ey2);
return h11 + h12 + h21 + h22;
}
static inline float wgs84_ellipsoid_to_geoid(float lat, float lon)
{
float x = (180.0f + DegOfRad(lon)) / 10.0f;