/*********************************************
*
* 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
{
public final byte GPS_FORMAT_DECIMAL=
0;
public final byte GPS_FORMAT_MINSEC=
1;
public final byte GPS_FORMAT_COUNT=
2;
byte act_gps_format=GPS_FORMAT_DECIMAL
;
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 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]<<8) |
(in_arr
[offset+
0]));
}
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];
// ground_speed 54 / 55
/*
if (version.compare(0,11)==version.VERSION_PREVIOUS)
{
TargetLongitude=parse_arr(8,in_arr);
TargetLatitude=parse_arr(12,in_arr);
Distance2Target=parse_arr(16,in_arr);
Angle2Target=parse_arr(20,in_arr);
Used_Sat=(byte)in_arr[24];
}
else
{
Longitude=parse_arr(0,in_arr);
Latitude=parse_arr(4,in_arr);
TargetLongitude=parse_arr(13,in_arr);
TargetLatitude=parse_arr(17,in_arr);
Distance2Target=-23 ; //parse_arr(16,in_arr);
Angle2Target=parse_arr(20,in_arr);
WayPointNumber=-1;
WayPointIndex=-1;
Used_Sat=(byte)in_arr[24];
}
*/
}
}