WebSVN - Projects - Diff - Rev 206 and 211 - /DUBwise/trunk/shared/src/MKGPSPosition.java


/*********************************************
 *                                            
 * class representing the DebugData Structure
 *                                            
 * Author:        Marcus -LiGi- Bueschleb    
 *
 * see README for further Infos
 *
 * Some code taken from here:
 * http://www.koders.com/java/fidFC75A641A87B51BB757E9CD3136C7886C491487F.aspx
 *
 * and
 * http://www.movable-type.co.uk/scripts/latlong.html
 *
 * thanx a lot for sharing!
 *
 ********************************************/
 
package org.ligi.ufo;
 
 
import java.lang.Math;
public class MKGPSPosition
    implements DUBwiseDefinitions
{
 
 
 
 
    public byte act_gps_format=GPS_FORMAT_DECIMAL;
    public byte act_speed_format=SPEED_FORMAT_KMH;
 
    public final static int MAX_WAYPOINTS=100;
 
    public int[] LongWP;
    public int[] LatWP;
    public String[] NameWP;
 
    int UBatt;
 
    public int last_wp=0;
 
    public int Longitude;
    public int Latitude;
    public int Altitude;
 
    public int TargetLongitude;
    public int TargetLatitude;
    public int TargetAltitude;
 
    public int HomeLongitude;
    public int HomeLatitude;
    public int HomeAltitude;
 
    public int Distance2Target;
    public int Angle2Target;
 
    public int Distance2Home;
    public int Angle2Home;
 
    public byte SatsInUse=-1;
    public byte WayPointNumber=-1;
    public byte WayPointIndex=-1;
       
    public int AngleNick = -1;
    public int AngleRoll = -1;
    public int SenderOkay = -1;
    public int MKFlags= -1;
    public int NCFlags= -1;
    public int ErrorCode= -1;
 
 
 
    public int Altimeter=-1; // hight according to air pressure
    public int Variometer=-1; // climb(+) and sink(-) rate
    public int FlyingTime=-1;
 
    public int GroundSpeed=-1;
    public int Heading=-1;
    public int CompasHeading=-1;
 
 
 
//#if cldc11=="on"
    public static final double PI = Math.PI;
    public static final double PI_div2 = PI / 2.0;
    public static final double PI_div4 = PI / 4.0;
    public static final double RADIANS = PI / 180.0;
    public static final double DEGREES = 180.0 / PI;
 
    private static final double p4 = 0.161536412982230228262e2;
    private static final double p3 = 0.26842548195503973794141e3;
    private static final double p2 = 0.11530293515404850115428136e4;
    private static final double p1 = 0.178040631643319697105464587e4;
    private static final double p0 = 0.89678597403663861959987488e3;
    private static final double q4 = 0.5895697050844462222791e2;
    private static final double q3 = 0.536265374031215315104235e3;
    private static final double q2 = 0.16667838148816337184521798e4;
    private static final double q1 = 0.207933497444540981287275926e4;
    private static final double q0 = 0.89678597403663861962481162e3;
 
 
 
    private static double _ATAN(double X)
    {
        if (X < 0.414213562373095048802)
            return _ATANX(X);
        else if (X > 2.414213562373095048802)
            return PI_div2 - _ATANX(1.0 / X);
        else
            return PI_div4 + _ATANX((X - 1.0) / (X + 1.0));
    }
 
 
    private static double _ATANX(double X)
    {
        double XX = X * X;
        return X * ((((p4 * XX + p3) * XX + p2) * XX + p1) * XX + p0)/ (((((XX + q4) * XX + q3) * XX + q2) * XX + q1) * XX + q0);
    }
 
 
 
    public double aTan2(double Y, double X)
    {
 
        if (X == 0.0) {
            if (Y > 0.0)
                return PI_div2;
     
            else if (Y < 0.0)
                return -PI_div2;
            else
                return 0.0;
        }
 
        // X<0
        else if (X < 0.0) {
            if (Y >= 0.0)
                return (PI - _ATAN(Y / -X)); // Y>=0,X<0 |Y/X|
            else
                return -(PI - _ATAN(Y / X)); // Y<0,X<0 |Y/X|
       
        }
 
        // X>0
        else if (X > 0.0)
            {
            if (Y > 0.0)
                return _ATAN(Y / X);
            else
                return -_ATAN(-Y / X);
           
            }
 
    return 0.0;
 
  }
 
    public int distance2wp(int id)
    {
        double lat1=(Latitude/10000000.0)*RADIANS;
        double long1=(Longitude/10000000.0)*RADIANS;
 
        double lat2=(LatWP[id]/10000000.0)*RADIANS;
        double long2=(LongWP[id]/10000000.0)*RADIANS;
 
 
        double dLat= (lat2-lat1);
        double dLon= (long2-long1);
 
        double a = Math.sin(dLat/2.0) * Math.sin(dLat/2.0) +
        Math.cos(lat1) * Math.cos(lat2) *
        Math.sin(dLon/2.0) * Math.sin(dLon/2.0);
 
        return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8);
    }
 
 
 
 
    public int angle2wp(int id)
    {
        // TODO reuse from distance
        double lat1=(Latitude/10000000.0)*RADIANS;
        double long1=(Longitude/10000000.0)*RADIANS;
 
        double lat2=(LatWP[id]/10000000.0)*RADIANS;
        double long2=(LongWP[id]/10000000.0)*RADIANS;
 
 
        double dLat= (lat2-lat1);
        double dLon= (long2-long1);
 
       
 
        double y = Math.sin(dLon) * Math.cos(lat2);
        double x = Math.cos(lat1)*Math.sin(lat2) -   Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
        return ((int)(aTan2(y, x)*DEGREES)+360)%360;
 
    }
 
 
//#endif
 
//#if cldc11!="on"
//#    public int distance2wp(int id)
//#    {
//#     return -1;
//#    }
 
//#    public int angle2wp(int id)
//#    {
//#     return -1;
//#    }
//#endif
 
    public void push_wp()
    {
        LongWP[last_wp]=Longitude;
        LatWP[last_wp]=Latitude;
 
        last_wp++;
    }
 
    /*    public void next_gps_format()
    {
        act_gps_format=(byte)((act_gps_format+1)%GPS_FORMAT_COUNT);
        }*/
 
    public String act_gps_format_str(int val)
    {
        switch(act_gps_format)
            {
            case GPS_FORMAT_DECIMAL:
                return "" + val/10000000 + "." +val%10000000  ;
            case GPS_FORMAT_MINSEC:
                return "" +  val/10000000 + "^" +  ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 +  "." + ((((val%10000000)*60)%10000000)*60)%10000000;
            default:
                return "invalid format";
            }
    }
 
   
 
    public String act_speed_format_str(int val)
    {
        switch(act_speed_format)
            {
            case SPEED_FORMAT_KMH:
                return "" +  ((((val*60)/100)*60)/1000) + " km/h";
 
            case SPEED_FORMAT_MPH:
                return "" +  (((((val*60)/100)*60)/1000)*10)/16 + " m/h";
 
            case SPEED_FORMAT_CMS:
                return "" + val+ " cm/s";
               
            default:
                return "invalid speed format";
            }
    }
 
    public String GroundSpeed_str()
    {
        return act_speed_format_str(GroundSpeed);
 
    }
 
    public String WP_Latitude_str(int id)
    {
       
        return act_gps_format_str(LatWP[id]); //+ "''N"  ;
    }
 
    public String WP_Longitude_str(int id)
    {
        return act_gps_format_str(LongWP[id]); //+ "''E"  ;
 
    }
 
    public String Latitude_str()
    {
 
        return act_gps_format_str(Latitude) ;
    }
 
    public String Longitude_str()
    {
        return act_gps_format_str(Longitude)  ;
 
    }
 
 
    public String TargetLatitude_str()
    {
        return act_gps_format_str(TargetLatitude) ;
    }
 
    public String TargetLongitude_str()
    {
        return act_gps_format_str(TargetLongitude)  ;
    }
 
    public String HomeLatitude_str()
    {
 
        return act_gps_format_str(HomeLatitude) ;
    }
 
    public String HomeLongitude_str()
    {
        return act_gps_format_str(HomeLongitude)  ;
    }
 
 
 
 
    // Constructor
    public MKGPSPosition()
    {
 
        LongWP=new int[MAX_WAYPOINTS];
        LatWP=new int[MAX_WAYPOINTS];
       
 
        NameWP=new String[MAX_WAYPOINTS];
        // predefined waypoints
 
        /*
        LongWP[0]=123230170;
        LatWP[0]= 513600170 ;
        NameWP[0]="Sicherer PC1";
 
        LongWP[1]=123269000;
        LatWP[1]= 513662670;
        NameWP[1]="Sicherer PC2";
 
        LongWP[2]=123475570;
        LatWP[2]= 513569750 ;
        NameWP[2]="Treffpunkt Seba";
        */
 
        last_wp=0;
    }
 
    private int parse_arr_4(int offset,int[] in_arr)
    {
        return ((in_arr[offset+3]<<24) |
                (in_arr[offset+2]<<16) |
                (in_arr[offset+1]<<8)  |
                (in_arr[offset+0]));
    }
 
 
    private int parse_arr_2(int offset,int[] in_arr)
    {
        return (((in_arr[offset+1]&0xFF)<<8)  |
                (in_arr[offset+0]&0xFF ));
    }
 
 
 
    public void set_by_mk_data(int[] in_arr,MKVersion version)
    {
        Longitude=parse_arr_4(0,in_arr);
        Latitude=parse_arr_4(4,in_arr);
        Altitude=parse_arr_4(8,in_arr);
        //status=in_arr[12];
 
        TargetLongitude=parse_arr_4(13,in_arr);
        TargetLatitude=parse_arr_4(17,in_arr);
        TargetAltitude=parse_arr_4(21,in_arr);
        //Targetstatus=in_arr[25];
 
        Distance2Target=parse_arr_2(26,in_arr);
        Angle2Target=parse_arr_2(28,in_arr);
 
        HomeLongitude=parse_arr_4(30,in_arr);
        HomeLatitude=parse_arr_4(34,in_arr);
        HomeAltitude=parse_arr_4(38,in_arr);
        //Targetstatus=in_arr[42];
 
        Distance2Home=parse_arr_2(43,in_arr);
        Angle2Home=parse_arr_2(45,in_arr);
 
        WayPointIndex=(byte)in_arr[47];
        WayPointNumber=(byte)in_arr[48];
 
        SatsInUse=(byte)in_arr[49];
       
       
        Altimeter=parse_arr_2(50,in_arr); // hight according to air pressure
        Variometer=parse_arr_2(52,in_arr);; // climb(+) and sink(-) rate
        FlyingTime=parse_arr_2(54,in_arr);;
       
        UBatt= in_arr[56];
 
 
        GroundSpeed= parse_arr_2(57,in_arr);
        Heading= parse_arr_2(59,in_arr);
        CompasHeading= parse_arr_2(61,in_arr);
       
        AngleNick = in_arr[63];
        AngleRoll = in_arr[64];
        SenderOkay = in_arr[65];
 
        MKFlags=in_arr[66];
        NCFlags=in_arr[67];
 
        ErrorCode=in_arr[67];
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
    }
 
 
 
}
 

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

Subversion Repositories Projects

(root)/DUBwise/trunk/shared/src/MKGPSPosition.java - Rev 206 → 211