You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

height.cc 4.1KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161
  1. /*
  2. Copyright (C) 2008 Alexander Stapff, a.stapff@gmx.de
  3. Geoid separation code by Oleg Gusev, from data by Peter Dana.
  4. This code was published by the gpsd project (http://gpsd.berlios.de/)
  5. under the BSD license.
  6. This program 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 of the License, or
  9. (at your option) any later version.
  10. This program is distributed in the hope that it will be useful,
  11. but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. GNU General Public License for more details.
  14. You should have received a copy of the GNU General Public License
  15. along with this program; if not, write to the Free Software
  16. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  17. */
  18. #include "defs.h"
  19. #include "filterdefs.h"
  20. #include <math.h>
  21. #include <stdlib.h>
  22. #define MYNAME "height"
  23. #if FILTERS_ENABLED
  24. static char* addopt = NULL;
  25. static char* wgs84tomslopt = NULL;
  26. static double addf;
  27. static
  28. arglist_t height_args[] = {
  29. {
  30. "add", &addopt, "Adds a constant value to every altitude (meter, append \"f\" (x.xxf) for feet)",
  31. NULL, ARGTYPE_BEGIN_REQ | ARGTYPE_FLOAT, ARG_NOMINMAX
  32. },
  33. {
  34. "wgs84tomsl", &wgs84tomslopt, "Converts WGS84 ellipsoidal height to orthometric height (MSL)",
  35. NULL, ARGTYPE_END_REQ | ARGTYPE_BOOL, ARG_NOMINMAX
  36. },
  37. ARG_TERMINATOR
  38. };
  39. static double bilinear(double x1, double y1, double x2, double y2, double x, double y, double z11, double z12, double z21, double z22)
  40. {
  41. double delta;
  42. if (y1 == y2 && x1 == x2) {
  43. return (z11);
  44. }
  45. if (y1 == y2 && x1 != x2) {
  46. return (z22*(x-x1)+z11*(x2-x))/(x2-x1);
  47. }
  48. if (x1 == x2 && y1 != y2) {
  49. return (z22*(y-y1)+z11*(y2-y))/(y2-y1);
  50. }
  51. delta=(y2-y1)*(x2-x1);
  52. return (z22*(y-y1)*(x-x1)+z12*(y2-y)*(x-x1)+z21*(y-y1)*(x2-x)+z11*(y2-y)*(x2-x))/delta;
  53. }
  54. /* return geoid separation (MSL - WGS84) in meters, given a lat/lot in degrees */
  55. static double wgs84_separation(double lat, double lon)
  56. {
  57. #include "height.h"
  58. int ilat, ilon;
  59. int ilat1, ilat2, ilon1, ilon2;
  60. /* sanity checks to prevent segfault on bad data */
  61. if ((lat > 90.0) || (lat < -90.0)) {
  62. fatal(MYNAME ": Invalid latitude value (%f)\n", lat);
  63. }
  64. if ((lon > 180.0) || (lon < -180.0)) {
  65. fatal(MYNAME ": Invalid longitude value (%f)\n", lon);
  66. }
  67. ilat=(int)floor((90.0+lat)/GEOID_GRID_DEG);
  68. ilon=(int)floor((180.0+lon)/GEOID_GRID_DEG);
  69. ilat1=ilat;
  70. ilon1=ilon;
  71. ilat2=(ilat < GEOID_ROW-1)? ilat+1:ilat;
  72. ilon2=(ilon < GEOID_COL-1)? ilon+1:ilon;
  73. return bilinear(
  74. ilon1*GEOID_GRID_DEG-180.0,ilat1*GEOID_GRID_DEG-90.0,
  75. ilon2*GEOID_GRID_DEG-180.0,ilat2*GEOID_GRID_DEG-90.0,
  76. lon, lat,
  77. (double)geoid_delta[ilon1+ilat1*GEOID_COL]/GEOID_SCALE,
  78. (double)geoid_delta[ilon2+ilat1*GEOID_COL]/GEOID_SCALE,
  79. (double)geoid_delta[ilon1+ilat2*GEOID_COL]/GEOID_SCALE,
  80. (double)geoid_delta[ilon2+ilat2*GEOID_COL]/GEOID_SCALE
  81. );
  82. }
  83. static void
  84. correct_height(const Waypoint* wpt)
  85. {
  86. Waypoint* waypointp = (Waypoint*) wpt;
  87. if (waypointp->altitude != unknown_alt) {
  88. if (addopt) {
  89. waypointp->altitude += addf;
  90. }
  91. if (wgs84tomslopt) {
  92. waypointp->altitude -= wgs84_separation(waypointp->latitude, waypointp->longitude);
  93. }
  94. }
  95. }
  96. static void
  97. height_init(const char* args)
  98. {
  99. char* unit;
  100. if (addopt) {
  101. addf = strtod(addopt, &unit);
  102. if (*unit == 'f' || *unit== 'F') {
  103. addf = FEET_TO_METERS(addf);
  104. } else if ((*unit != 'm') && (*unit != 'M') && (*unit != '\0')) {
  105. fatal(MYNAME ": Invalid unit (\"%c\")! Please use \"m\" for meter or \"f\" for feet.\n", *unit);
  106. }
  107. } else {
  108. addf = 0.0;
  109. }
  110. }
  111. static void
  112. height_process(void) /* this procedure must be present in vecs */
  113. {
  114. waypt_disp_all(correct_height);
  115. route_disp_all(NULL, NULL, correct_height);
  116. track_disp_all(NULL, NULL, correct_height);
  117. }
  118. filter_vecs_t height_vecs = {
  119. height_init,
  120. height_process,
  121. NULL,
  122. NULL,
  123. height_args
  124. };
  125. #endif // FILTERS_ENABLED