WebSVN - Projects - Diff - Rev 266 and 381 - /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= 0;
 
 
 
    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 gps_format_str(int val,int format)
    {
        switch(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" + act_gps_format;
            }
    }
    public String act_gps_format_str(int val)
    {
        return gps_format_str(val,act_gps_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)
    {
        int off=0;
        if (version.proto_minor>0) // fixme
            off++;
        Longitude=parse_arr_4(off+0,in_arr);
        Latitude=parse_arr_4(off+4,in_arr);
        Altitude=parse_arr_4(off+8,in_arr);
        //status=in_arr[12];
 
        TargetLongitude=parse_arr_4(off+13,in_arr);
        TargetLatitude=parse_arr_4(off+17,in_arr);
        TargetAltitude=parse_arr_4(off+21,in_arr);
        //Targetstatus=in_arr[25];
 
        Distance2Target=parse_arr_2(off+26,in_arr);
        Angle2Target=parse_arr_2(off+28,in_arr);
 
        HomeLongitude=parse_arr_4(off+30,in_arr);
        HomeLatitude=parse_arr_4(off+34,in_arr);
        HomeAltitude=parse_arr_4(off+38,in_arr);
        //Targetstatus=in_arr[42];
 
        Distance2Home=parse_arr_2(off+43,in_arr);
        Angle2Home=parse_arr_2(off+45,in_arr);
 
        WayPointIndex=(byte)in_arr[off+47];
        WayPointNumber=(byte)in_arr[off+48];
 
        SatsInUse=(byte)in_arr[off+49];
       
       
        Altimeter=parse_arr_2(off+50,in_arr); // hight according to air pressure
        Variometer=parse_arr_2(off+52,in_arr);; // climb(+) and sink(-) rate
        FlyingTime=parse_arr_2(off+54,in_arr);;
       
        UBatt= in_arr[off+56];
 
 
        GroundSpeed= parse_arr_2(off+57,in_arr);
        Heading= parse_arr_2(off+59,in_arr);
        CompasHeading= parse_arr_2(off+61,in_arr);
       
        AngleNick = in_arr[off+63];
        AngleRoll = in_arr[off+64];
        SenderOkay = in_arr[off+65];
 
        MKFlags=in_arr[off+66];
        NCFlags=in_arr[off+67];
 
        ErrorCode=in_arr[off+68];
 
 
    }
 
 
 
}
 

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

Subversion Repositories Projects

(root)/DUBwise/trunk/shared/src/MKGPSPosition.java - Rev 266 → 381