~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/novatelINS/src/gps_convert.cpp

  • Committer: Nick Hillier
  • Date: 2011-08-03 03:16:26 UTC
  • Revision ID: nick.hillier@csiro.au-20110803031626-3ekp14jxlbda9bzf
Initial add of ROS wrappers for novatel INS around the Orca driver.

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
/*
 
2
 * Author Hendrik Erckens, Adrian Bonchis and Nick Hillier
 
3
 * 
 
4
 * Implementation of Redfearn's formula available from:
 
5
 * http://www.ga.gov.au/cedda/applications/458 (accessed 6/07/2011)
 
6
 * (Not sure how generic this is, or if it's Australia or Southern 
 
7
 * Hemisphere only)
 
8
 * 
 
9
 */
 
10
/*
 
11
 * This program 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 Lesser General Public License for more details.
 
15
 * 
 
16
 * You should have received a copy of the GNU Lesser General Public License
 
17
 * along with this program.  If not, see <http://www.gnu.org/licenses/>. 
 
18
 */
 
19
 
 
20
 
 
21
#include <math.h>                                                                                                                                            
 
22
 
 
23
 
 
24
/**
 
25
 * convert GPS latitude and longitude to Easting and Northing
 
26
 */
 
27
#define a_sma 6378137.00        /* semi-major axis */
 
28
#define f (1/298.257222101)        /* flattening, GRS80 */
 
29
#define ex2 (2.0*f - pow(f,2))  /* eccentricity powers (2, 4, 6) */
 
30
#define ex4 (ex2*ex2)
 
31
#define ex6 (ex4*ex2)
 
32
#define False_east 500000.0     /* false easting (m) */
 
33
#define False_north 10000000.0      /* false northing (m) */
 
34
#define K0 0.9996           /* central scale factor */
 
35
 
 
36
 
 
37
int latlong2eastnorth(double latitude, double longitude, double *easting, double *northing, int *zone)
 
38
{
 
39
        double phi = latitude;
 
40
        double lambda = longitude;
 
41
        double coslat1, coslat2, coslat3, coslat4, coslat5, coslat6, coslat7, coslat8;
 
42
        double A0, A2, A4, A6, m;
 
43
        double rho, niu, psi, psi1, psi2, psi3, psi4;
 
44
        double C23, C24, C25, C26, L4, C5, C6, C7, w;
 
45
        double w1, w2, w3, w4, w5, w6, w7, w8; 
 
46
        double tan1, tan2, tan4, tan6;
 
47
        double Term1, Term2, Term3, Term4, SumE, E1, SumN, N1;
 
48
        double Easting, Northing;
 
49
 
 
50
        
 
51
        phi = phi * M_PI / 180.0;
 
52
 
 
53
        coslat1 = cos(phi);
 
54
        coslat2 = coslat1*coslat1;
 
55
        coslat3 = coslat2*coslat1;
 
56
        coslat4 = coslat3*coslat1;
 
57
        coslat5 = coslat4*coslat1;
 
58
        coslat6 = coslat5*coslat1;
 
59
        coslat7 = coslat6*coslat1;
 
60
        coslat8 = coslat7*coslat1;
 
61
 
 
62
 
 
63
        // meridian distance
 
64
        A0 = 1.0 - (ex2 / 4.0) - (3.0 * ex4 / 64.0) - (5.0 * ex6 / 256.0);
 
65
        A2 = (3.0/8.0)*(ex2+ex4/4.0+15.0*ex6/128.0);
 
66
        A4 = (15.0/256.0)*(ex4+3.0*ex6/4.0);
 
67
        A6 = 35.0*ex6/3072.0;
 
68
 
 
69
        m = a_sma*(A0*phi - A2*sin(2.0*phi) + A4*sin(4.0*phi) - A6*sin(6.0*phi));
 
70
 
 
71
        // radius of curvature for given latitude
 
72
        rho = a_sma*(1.0-ex2)/pow((1.0-ex2*sin(phi)*sin(phi)),1.5);
 
73
        niu = a_sma/pow((1.0-ex2*sin(phi)*sin(phi)),0.5);
 
74
        psi = niu/rho;
 
75
 
 
76
        psi1 = psi;
 
77
        psi2 = psi1*psi1;
 
78
        psi3 = psi2*psi1;
 
79
        psi4 = psi3*psi1;
 
80
 
 
81
        C23 = 6.0; // Zone width (degrees)
 
82
        C24 = -177.0; // Meridian of zone 1 (degrees)
 
83
        C25 = C24 - 1.5*C23; // Longitude of Western Edge of Zone 0
 
84
        C26 = C25 + 0.5*C23; // Central Meridian of Zone 0
 
85
        L4 = lambda; // degrees in decimal format
 
86
        C5 = (L4-C25)/C23; // Zone as a real number
 
87
        C7 = floor(C5); // Zone number
 
88
        C6 = C7*C23+C26; // Central Meridian
 
89
        w = L4 - C6;
 
90
 
 
91
        w1 = w*M_PI/180.0;
 
92
        w2 = w1*w1;
 
93
        w3 = w2*w1;
 
94
        w4 = w3*w1;
 
95
        w5 = w4*w1;
 
96
        w6 = w5*w1;
 
97
        w7 = w6*w1;
 
98
        w8 = w7*w1;
 
99
 
 
100
        tan1 = tan(phi);
 
101
        tan2 = tan1*tan1;
 
102
        tan4 = tan2*tan2;
 
103
        tan6 = tan4*tan2;
 
104
 
 
105
        Term1 = niu*w1*coslat1;
 
106
        Term2 = niu*w1*coslat1*(w2/6.0)*coslat2*(psi1-tan2);
 
107
        Term3 = niu*w1*coslat1*(w4/120.0)*coslat4*(4.0*psi3*(1.0-6.0*tan2) + psi2*(1.0+8.0*tan2) - psi1*2.0*tan2 +tan4);
 
108
        Term4 = niu*w1*coslat1*(w6/5040.0)*coslat6*(61.0 - 479.0*tan2 + 179.0*tan4 - tan6);
 
109
        SumE = Term1+Term2+Term3+Term4;
 
110
        E1 = K0*SumE;
 
111
        Easting = E1 + False_east;
 
112
 
 
113
        Term1 = (w2/2.0)*niu*sin(phi)*coslat1;
 
114
        Term2 = (w4/24.0)*niu*sin(phi)*coslat3*(4.0*psi2+psi1-tan2);
 
115
        Term3 = (w6/720.0)*niu*sin(phi)*coslat5*(8.0*psi4*(11.0-24.0*tan2)-28.0*psi3*(1.0-6.0*tan2)+psi2*(1.0-32.0*tan2)-psi*2.0*tan2+tan4);
 
116
        Term4 = (w8/40320.0)*niu*sin(phi)*coslat7*(1385.0-3111.0*tan2+534.0*tan4-tan6);
 
117
        SumN = m+Term1+Term2+Term3+Term4;
 
118
        N1 = K0*SumN;
 
119
        Northing = N1+False_north;
 
120
 
 
121
        /* return calc values */
 
122
        *(northing) = Northing;
 
123
        *(easting) = Easting;
 
124
        *(zone) = C7;
 
125
 
 
126
        return 0;
 
127
}