1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
|
/*
Copyright (C) 2008 Alexander Stapff, a.stapff@gmx.de
Geoid separation code by Oleg Gusev, from data by Peter Dana.
This code was published by the gpsd project (http://gpsd.berlios.de/)
under the BSD license.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include "defs.h"
#include "height.h"
#include <cmath> // for floor
#include <cstdint> // for int8_t
#include <cstdlib> // for strtod
#define MYNAME "height"
#if FILTERS_ENABLED
// Until c++17 we have to define odr-used constexpr static data members at namespace scope.
#if __cplusplus < 201703L
constexpr double HeightFilter::geoid_grid_deg;
constexpr double HeightFilter::geoid_scale;
constexpr int HeightFilter::geoid_row;
constexpr int HeightFilter::geoid_col;
constexpr int8_t HeightFilter::geoid_delta[geoid_row][geoid_col];
#endif
double HeightFilter::bilinear(double x1, double y1, double x2, double y2, double x, double y, double z11, double z12, double z21, double z22)
{
if (y1 == y2 && x1 == x2) {
return (z11);
}
if (y1 == y2 && x1 != x2) {
return (z22*(x-x1)+z11*(x2-x))/(x2-x1);
}
if (x1 == x2 && y1 != y2) {
return (z22*(y-y1)+z11*(y2-y))/(y2-y1);
}
double delta = (y2-y1)*(x2-x1);
return (z22*(y-y1)*(x-x1)+z12*(y2-y)*(x-x1)+z21*(y-y1)*(x2-x)+z11*(y2-y)*(x2-x))/delta;
}
/* return geoid separation (MSL - WGS84) in meters, given a lat/lot in degrees */
double HeightFilter::wgs84_separation(double lat, double lon)
{
/* sanity checks to prevent segfault on bad data */
if ((lat > 90.0) || (lat < -90.0)) {
fatal(MYNAME ": Invalid latitude value (%f)\n", lat);
}
if ((lon > 180.0) || (lon < -180.0)) {
fatal(MYNAME ": Invalid longitude value (%f)\n", lon);
}
auto ilat = static_cast<int>(floor((90.0+lat)/geoid_grid_deg));
auto ilon = static_cast<int>(floor((180.0+lon)/geoid_grid_deg));
int ilat1 = ilat;
int ilon1 = ilon;
int ilat2 = (ilat < geoid_row-1)? ilat+1:ilat;
int ilon2 = (ilon < geoid_col-1)? ilon+1:ilon;
return bilinear(
ilon1*geoid_grid_deg-180.0, ilat1*geoid_grid_deg-90.0,
ilon2*geoid_grid_deg-180.0, ilat2*geoid_grid_deg-90.0,
lon, lat,
static_cast<double>(geoid_delta[ilat1][ilon1])/geoid_scale,
static_cast<double>(geoid_delta[ilat1][ilon2])/geoid_scale,
static_cast<double>(geoid_delta[ilat2][ilon1])/geoid_scale,
static_cast<double>(geoid_delta[ilat2][ilon2])/geoid_scale
);
}
void HeightFilter::correct_height(const Waypoint* wpt)
{
auto* waypointp = const_cast<Waypoint*>(wpt);
if (waypointp->altitude != unknown_alt) {
if (addopt != nullptr) {
waypointp->altitude += addf;
}
if (wgs84tomslopt != nullptr) {
waypointp->altitude -= wgs84_separation(waypointp->latitude, waypointp->longitude);
}
}
}
void HeightFilter::init()
{
char* unit;
if (addopt != nullptr) {
addf = strtod(addopt, &unit);
if (*unit == 'f' || *unit== 'F') {
addf = FEET_TO_METERS(addf);
} else if ((*unit != 'm') && (*unit != 'M') && (*unit != '\0')) {
fatal(MYNAME ": Invalid unit (\"%c\")! Please use \"m\" for meter or \"f\" for feet.\n", *unit);
}
} else {
addf = 0.0;
}
}
void HeightFilter::process()
{
WayptFunctor<HeightFilter> correct_height_f(this, &HeightFilter::correct_height);
waypt_disp_all(correct_height_f);
route_disp_all(nullptr, nullptr, correct_height_f);
track_disp_all(nullptr, nullptr, correct_height_f);
}
#endif // FILTERS_ENABLED
|
