Subversion Repositories Projects

Rev

Rev 266 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
206 ligi 1
/*********************************************
2
 *                                            
3
 * class representing the DebugData Structure
4
 *                                            
5
 * Author:        Marcus -LiGi- Bueschleb    
6
 *
7
 * see README for further Infos
8
 *
9
 * Some code taken from here:
10
 * http://www.koders.com/java/fidFC75A641A87B51BB757E9CD3136C7886C491487F.aspx
11
 *
12
 * and
13
 * http://www.movable-type.co.uk/scripts/latlong.html
14
 *
15
 * thanx a lot for sharing!
16
 *
17
 ********************************************/
18
 
19
package org.ligi.ufo;
20
 
21
 
22
import java.lang.Math;
23
public class MKGPSPosition
211 ligi 24
    implements DUBwiseDefinitions
206 ligi 25
{
26
 
211 ligi 27
    public byte act_gps_format=GPS_FORMAT_DECIMAL;
28
    public byte act_speed_format=SPEED_FORMAT_KMH;
206 ligi 29
 
30
    public final static int MAX_WAYPOINTS=100;
31
 
32
    public int[] LongWP;
33
    public int[] LatWP;
34
    public String[] NameWP;
35
 
36
    int UBatt;
37
 
38
    public int last_wp=0;
39
 
40
    public int Longitude;
41
    public int Latitude;
42
    public int Altitude;
43
 
44
    public int TargetLongitude;
45
    public int TargetLatitude;
46
    public int TargetAltitude;
47
 
48
    public int HomeLongitude;
49
    public int HomeLatitude;
50
    public int HomeAltitude;
51
 
52
    public int Distance2Target;
53
    public int Angle2Target;
54
 
55
    public int Distance2Home;
56
    public int Angle2Home;
57
 
58
    public byte SatsInUse=-1;
59
    public byte WayPointNumber=-1;
60
    public byte WayPointIndex=-1;
61
 
62
    public int AngleNick = -1;
63
    public int AngleRoll = -1;
64
    public int SenderOkay = -1;
65
    public int MKFlags= -1;
66
    public int NCFlags= -1;
266 ligi 67
    public int ErrorCode= 0;
206 ligi 68
 
69
 
70
 
71
    public int Altimeter=-1; // hight according to air pressure
72
    public int Variometer=-1; // climb(+) and sink(-) rate
73
    public int FlyingTime=-1;
74
 
75
    public int GroundSpeed=-1;
76
    public int Heading=-1;
77
    public int CompasHeading=-1;
78
 
79
 
80
 
81
//#if cldc11=="on"
82
    public static final double PI = Math.PI;
83
    public static final double PI_div2 = PI / 2.0;
84
    public static final double PI_div4 = PI / 4.0;
85
    public static final double RADIANS = PI / 180.0;
86
    public static final double DEGREES = 180.0 / PI;
87
 
88
    private static final double p4 = 0.161536412982230228262e2;
89
    private static final double p3 = 0.26842548195503973794141e3;
90
    private static final double p2 = 0.11530293515404850115428136e4;
91
    private static final double p1 = 0.178040631643319697105464587e4;
92
    private static final double p0 = 0.89678597403663861959987488e3;
93
    private static final double q4 = 0.5895697050844462222791e2;
94
    private static final double q3 = 0.536265374031215315104235e3;
95
    private static final double q2 = 0.16667838148816337184521798e4;
96
    private static final double q1 = 0.207933497444540981287275926e4;
97
    private static final double q0 = 0.89678597403663861962481162e3;
98
 
99
 
100
 
101
    private static double _ATAN(double X)
102
    {
103
        if (X < 0.414213562373095048802)
104
            return _ATANX(X);
105
        else if (X > 2.414213562373095048802)
106
            return PI_div2 - _ATANX(1.0 / X);
107
        else
108
            return PI_div4 + _ATANX((X - 1.0) / (X + 1.0));
109
    }
110
 
111
 
112
    private static double _ATANX(double X)
113
    {
114
        double XX = X * X;
115
        return X * ((((p4 * XX + p3) * XX + p2) * XX + p1) * XX + p0)/ (((((XX + q4) * XX + q3) * XX + q2) * XX + q1) * XX + q0);
116
    }
117
 
118
 
119
 
120
    public double aTan2(double Y, double X)
121
    {
122
 
123
        if (X == 0.0) {
124
            if (Y > 0.0)
125
                return PI_div2;
126
 
127
            else if (Y < 0.0)
128
                return -PI_div2;
129
            else
130
                return 0.0;
131
        }
132
 
133
        // X<0
134
        else if (X < 0.0) {
135
            if (Y >= 0.0)
136
                return (PI - _ATAN(Y / -X)); // Y>=0,X<0 |Y/X|
137
            else
138
                return -(PI - _ATAN(Y / X)); // Y<0,X<0 |Y/X|
139
 
140
        }
141
 
142
        // X>0
143
        else if (X > 0.0)
144
            {
145
            if (Y > 0.0)
146
                return _ATAN(Y / X);
147
            else
148
                return -_ATAN(-Y / X);
149
 
150
            }
151
 
152
    return 0.0;
153
 
154
  }
155
 
156
    public int distance2wp(int id)
157
    {
158
        double lat1=(Latitude/10000000.0)*RADIANS;
159
        double long1=(Longitude/10000000.0)*RADIANS;
160
 
161
        double lat2=(LatWP[id]/10000000.0)*RADIANS;
162
        double long2=(LongWP[id]/10000000.0)*RADIANS;
163
 
164
 
165
        double dLat= (lat2-lat1);
166
        double dLon= (long2-long1);
167
 
168
        double a = Math.sin(dLat/2.0) * Math.sin(dLat/2.0) +
169
        Math.cos(lat1) * Math.cos(lat2) *
170
        Math.sin(dLon/2.0) * Math.sin(dLon/2.0);
171
 
172
        return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8);
173
    }
174
 
175
 
176
 
177
 
178
    public int angle2wp(int id)
179
    {
180
        // TODO reuse from distance
181
        double lat1=(Latitude/10000000.0)*RADIANS;
182
        double long1=(Longitude/10000000.0)*RADIANS;
183
 
184
        double lat2=(LatWP[id]/10000000.0)*RADIANS;
185
        double long2=(LongWP[id]/10000000.0)*RADIANS;
186
 
187
 
188
        double dLat= (lat2-lat1);
189
        double dLon= (long2-long1);
190
 
191
 
192
 
193
        double y = Math.sin(dLon) * Math.cos(lat2);
194
        double x = Math.cos(lat1)*Math.sin(lat2) -   Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
195
        return ((int)(aTan2(y, x)*DEGREES)+360)%360;
196
 
197
    }
198
 
199
 
200
//#endif
201
 
202
//#if cldc11!="on"
203
//#    public int distance2wp(int id)
204
//#    {
205
//#     return -1;
206
//#    }
207
 
208
//#    public int angle2wp(int id)
209
//#    {
210
//#     return -1;
211
//#    }
212
//#endif
213
 
214
    public void push_wp()
215
    {
216
        LongWP[last_wp]=Longitude;
217
        LatWP[last_wp]=Latitude;
218
 
219
        last_wp++;
220
    }
221
 
211 ligi 222
    /*    public void next_gps_format()
206 ligi 223
    {
224
        act_gps_format=(byte)((act_gps_format+1)%GPS_FORMAT_COUNT);
211 ligi 225
        }*/
206 ligi 226
 
381 ligi 227
 
228
 
229
    public String gps_format_str(int val,int format)
206 ligi 230
    {
381 ligi 231
        switch(format)
206 ligi 232
            {
233
            case GPS_FORMAT_DECIMAL:
234
                return "" + val/10000000 + "." +val%10000000  ;
235
            case GPS_FORMAT_MINSEC:
236
                return "" +  val/10000000 + "^" +  ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 +  "." + ((((val%10000000)*60)%10000000)*60)%10000000;
237
            default:
266 ligi 238
                return "invalid format" + act_gps_format;
206 ligi 239
            }
240
    }
381 ligi 241
    public String act_gps_format_str(int val)
242
    {
243
        return gps_format_str(val,act_gps_format);
206 ligi 244
 
381 ligi 245
    }
246
 
211 ligi 247
 
248
 
249
    public String act_speed_format_str(int val)
250
    {
251
        switch(act_speed_format)
252
            {
253
            case SPEED_FORMAT_KMH:
254
                return "" +  ((((val*60)/100)*60)/1000) + " km/h";
255
 
256
            case SPEED_FORMAT_MPH:
257
                return "" +  (((((val*60)/100)*60)/1000)*10)/16 + " m/h";
258
 
259
            case SPEED_FORMAT_CMS:
260
                return "" + val+ " cm/s";
261
 
262
            default:
263
                return "invalid speed format";
264
            }
265
    }
266
 
267
    public String GroundSpeed_str()
268
    {
269
        return act_speed_format_str(GroundSpeed);
270
 
271
    }
272
 
206 ligi 273
    public String WP_Latitude_str(int id)
274
    {
275
 
276
        return act_gps_format_str(LatWP[id]); //+ "''N"  ;
277
    }
278
 
279
    public String WP_Longitude_str(int id)
280
    {
281
        return act_gps_format_str(LongWP[id]); //+ "''E"  ;
282
 
283
    }
284
 
285
    public String Latitude_str()
286
    {
287
        return act_gps_format_str(Latitude) ;
288
    }
289
 
290
    public String Longitude_str()
291
    {
292
        return act_gps_format_str(Longitude)  ;
293
    }
294
 
295
 
296
    public String TargetLatitude_str()
297
    {
298
        return act_gps_format_str(TargetLatitude) ;
299
    }
300
 
301
    public String TargetLongitude_str()
302
    {
303
        return act_gps_format_str(TargetLongitude)  ;
304
    }
305
 
306
    public String HomeLatitude_str()
307
    {
308
        return act_gps_format_str(HomeLatitude) ;
309
    }
310
 
311
    public String HomeLongitude_str()
312
    {
313
        return act_gps_format_str(HomeLongitude)  ;
314
    }
315
 
316
 
317
    // Constructor
318
    public MKGPSPosition()
319
    {
320
 
321
        LongWP=new int[MAX_WAYPOINTS];
322
        LatWP=new int[MAX_WAYPOINTS];
323
 
324
 
325
        NameWP=new String[MAX_WAYPOINTS];
326
        // predefined waypoints
327
 
328
        /*
329
        LongWP[0]=123230170;
330
        LatWP[0]= 513600170 ;
331
        NameWP[0]="Sicherer PC1";
332
 
333
        LongWP[1]=123269000;
334
        LatWP[1]= 513662670;
335
        NameWP[1]="Sicherer PC2";
336
 
337
        LongWP[2]=123475570;
338
        LatWP[2]= 513569750 ;
339
        NameWP[2]="Treffpunkt Seba";
340
        */
341
 
342
        last_wp=0;
343
    }
344
    private int parse_arr_4(int offset,int[] in_arr)
345
    {
346
        return ((in_arr[offset+3]<<24) |
347
                (in_arr[offset+2]<<16) |
348
                (in_arr[offset+1]<<8)  |
349
                (in_arr[offset+0]));
350
    }
351
 
352
    private int parse_arr_2(int offset,int[] in_arr)
353
    {
211 ligi 354
        return (((in_arr[offset+1]&0xFF)<<8)  |
355
                (in_arr[offset+0]&0xFF ));
206 ligi 356
    }
357
 
358
 
359
 
360
    public void set_by_mk_data(int[] in_arr,MKVersion version)
361
    {
381 ligi 362
        int off=0;
363
        if (version.proto_minor>0) // fixme
364
            off++;
365
        Longitude=parse_arr_4(off+0,in_arr);
366
        Latitude=parse_arr_4(off+4,in_arr);
367
        Altitude=parse_arr_4(off+8,in_arr);
206 ligi 368
        //status=in_arr[12];
369
 
381 ligi 370
        TargetLongitude=parse_arr_4(off+13,in_arr);
371
        TargetLatitude=parse_arr_4(off+17,in_arr);
372
        TargetAltitude=parse_arr_4(off+21,in_arr);
206 ligi 373
        //Targetstatus=in_arr[25];
374
 
381 ligi 375
        Distance2Target=parse_arr_2(off+26,in_arr);
376
        Angle2Target=parse_arr_2(off+28,in_arr);
206 ligi 377
 
381 ligi 378
        HomeLongitude=parse_arr_4(off+30,in_arr);
379
        HomeLatitude=parse_arr_4(off+34,in_arr);
380
        HomeAltitude=parse_arr_4(off+38,in_arr);
206 ligi 381
        //Targetstatus=in_arr[42];
382
 
381 ligi 383
        Distance2Home=parse_arr_2(off+43,in_arr);
384
        Angle2Home=parse_arr_2(off+45,in_arr);
206 ligi 385
 
381 ligi 386
        WayPointIndex=(byte)in_arr[off+47];
387
        WayPointNumber=(byte)in_arr[off+48];
206 ligi 388
 
381 ligi 389
        SatsInUse=(byte)in_arr[off+49];
206 ligi 390
 
391
 
381 ligi 392
        Altimeter=parse_arr_2(off+50,in_arr); // hight according to air pressure
393
        Variometer=parse_arr_2(off+52,in_arr);; // climb(+) and sink(-) rate
394
        FlyingTime=parse_arr_2(off+54,in_arr);;
206 ligi 395
 
381 ligi 396
        UBatt= in_arr[off+56];
206 ligi 397
 
211 ligi 398
 
381 ligi 399
        GroundSpeed= parse_arr_2(off+57,in_arr);
400
        Heading= parse_arr_2(off+59,in_arr);
401
        CompasHeading= parse_arr_2(off+61,in_arr);
206 ligi 402
 
381 ligi 403
        AngleNick = in_arr[off+63];
404
        AngleRoll = in_arr[off+64];
405
        SenderOkay = in_arr[off+65];
206 ligi 406
 
381 ligi 407
        MKFlags=in_arr[off+66];
408
        NCFlags=in_arr[off+67];
206 ligi 409
 
381 ligi 410
        ErrorCode=in_arr[off+68];
206 ligi 411
 
412
 
413
    }
414
 
415
 
416
 
417
}