Devuan fork of gpsd
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.
 
 
 
 
 
 

107 lines
2.1 KiB

  1. /*
  2. * This file is Copyright (c) 2010-2018 by the GPSD project
  3. * SPDX-License-Identifier: BSD-2-clause
  4. */
  5. #include <sys/types.h>
  6. #include <stdio.h>
  7. #include <string.h>
  8. #include <stdlib.h>
  9. #include <math.h>
  10. extern char *__progname;
  11. double A = 6378137.0, B = 6356752.3142;
  12. static double rad2deg(double r){
  13. return (180.0 * r / M_PI);
  14. }
  15. static double deg2rad(double r){
  16. return (M_PI * r / 180.0);
  17. }
  18. static void lla2ecef(double *lla, double *ecef){
  19. double N;
  20. lla[0] = deg2rad(lla[0]);
  21. lla[1] = deg2rad(lla[1]);
  22. N = pow(A,2) / sqrt(
  23. pow(A,2) * pow(cos(lla[0]),2) +
  24. pow(B,2) * pow(sin(lla[0]),2)
  25. );
  26. ecef[0] = (N + lla[2]) * cos(lla[0]) * cos(lla[1]);
  27. ecef[1] = (N + lla[2]) * cos(lla[0]) * sin(lla[1]);
  28. ecef[2] = (pow(B,2) / pow(A,2) * N + lla[2]) * sin(lla[0]);
  29. return;
  30. }
  31. static void ecef2lla(double *ecef, double *lla){
  32. double E, F, N, P, T;
  33. E = (pow(A,2) - pow(B,2)) / pow(A,2);
  34. F = (pow(A,2) - pow(B,2)) / pow(B,2);
  35. P = sqrt(pow(ecef[0], 2) + pow(ecef[1],2));
  36. T = atan((ecef[2] * A)/(P * B));
  37. lla[0] = atan(
  38. (ecef[2] + F * B * pow(sin(T), 3) ) /
  39. (P - E * A * pow(cos(T), 3) )
  40. );
  41. lla[1] = atan( ecef[1] / ecef[0] );
  42. N = pow(A,2) / sqrt(
  43. pow(A,2) * pow(cos(lla[0]),2) +
  44. pow(B,2) * pow(sin(lla[0]),2)
  45. );
  46. lla[2] = P / cos(lla[0]) - N;
  47. lla[1] = rad2deg(lla[1])-180;
  48. lla[0] = rad2deg(lla[0]);
  49. return;
  50. }
  51. int
  52. main(int argc, char **argv){
  53. double ecef[3], lla[3];
  54. if (4 != argc){
  55. printf("usage:\tecef2lla X Y Z\n\tlla2ecef lat lon alt\n");
  56. return 1;
  57. }
  58. if (0 == strcmp(__progname, "lla2ecef")){
  59. /* Edmonton:
  60. lla[0] = 53.52716;
  61. lla[1] = -113.53013;
  62. lla[2] = 707.0;
  63. */
  64. lla[0] = atof(argv[1]);
  65. lla[1] = atof(argv[2]);
  66. lla[2] = atof(argv[3]);
  67. lla2ecef(lla, ecef);
  68. printf("%.2lf %.2lf %.2lf\n", ecef[0], ecef[1], ecef[2]);
  69. }
  70. if (0 == strcmp(__progname, "ecef2lla")){
  71. /* Edmonton:
  72. ecef[0] = -1517110.0;
  73. ecef[1] = -3484096.0;
  74. ecef[2] = 5106188.0;
  75. */
  76. ecef[0] = atof(argv[1]);
  77. ecef[1] = atof(argv[2]);
  78. ecef[2] = atof(argv[3]);
  79. ecef2lla(ecef, lla);
  80. printf("%.7lf %.7lf %.2lf\n", lla[0], lla[1], lla[2]);
  81. }
  82. return 0;
  83. }