Subversion Repositories Projects

Rev

Rev 266 | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*********************************************
 *                                            
 * 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];


    }



}