Subversion Repositories Projects

/*

// definiert in: mk_data-stucts.h

typedef struct

{

    u16 Distance;       // distance to target in cm

    s16 Bearing;        // course to target in deg

} __attribute__((packed)) GPS_PosDev_t;

*/

/*

// definiert in: mk_data-stucts.h

typedef struct

{

    s32 Longitude;      // in 1E-7 deg

    s32 Latitude;       // in 1E-7 deg

    s32 Altitude;       // in mm

    u8 Status;          // validity of data

} __attribute__((packed)) GPS_Pos_t;

*/

}

///**

// * \brief Calculate distance between two points

// * \return Distance in meters

// */

//int32_t nmea_distance(

//        const nmeaPOS *from_pos,    /**< From position in radians */

//        const nmeaPOS *to_pos       /**< To position in radians */

//        )

//{

//  int32_t dist = ((int32_t)NMEA_EARTHRADIUS_M) * acos(

//        sin(to_pos->lat) * sin(from_pos->lat) +

//        cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)

//        );

//    return dist;

//}

//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)

//// zur aktuellen Position(nach Motorstart)

//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)

//{ double lat1, lon1, lat2, lon2, d1, dlat;

//        geo_t geo;

//

//        lon1 = MK_pos.Home_Lon;

//        lat1 = MK_pos.Home_Lat;

//        lon2 = (double)pos->Longitude   / 10000000.0;

//        lat2 = (double)pos->Latitude    / 10000000.0;

//

//        // Formel verwendet von http://www.kompf.de/gps/distcalc.html

//        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator

//        // es wird jedoch in Meter weiter gerechnet

//        d1       = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);

//        dlat = 111300 * (double)(lat1 - lat2);

//        // returns a value in metres http://www.kompf.de/gps/distcalc.html

//        geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit

//        if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400

//        geo.distance = sqrt(d1 * d1 + dlat * dlat);

//        return(geo);

//}

// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)

// zur aktuellen Position(nach Motorstart)

//--------------------------------------------------------------

//--------------------------------------------------------------

/*

geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos )

{

    int32_t lat1, lon1, lat2, lon2;

        int32_t d1, dlat;

        geo_t geo;

        lon1 = home->Home_Lon;

        lat1 = home->Home_Lat;

        lon2 = pos->Longitude;

        lat2 = pos->Latitude;

        if( !CheckGPS )

        {

            writex_gpspos(  0, 3, home->Home_Lat , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)

            writex_gpspos( 11, 3, home->Home_Lon , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)

            writex_gpspos(  0, 4, pos->Latitude  , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)

            writex_gpspos( 11, 4, pos->Longitude , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)

            //lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0);     // 30.05.2014 OG: my_itoa() gibt es nicht mehr

            //lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr

            //lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0);      // 30.05.2014 OG: my_itoa() gibt es nicht mehr

            //lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr

        }

        // Formel verwendet von http://www.kompf.de/gps/distcalc.html

        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator

        // es wird jedoch in dm Meter weiter gerechnet

        // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1

        d1       = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;

        dlat = 1113 * (lat1 - lat2) / 10000;

        geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit

        geo.distance = sqrt32(d1 * d1 + dlat * dlat);

        if( !CheckGPS )

        {

            lcd_printp_at (0, 5, PSTR("Bear:"), 0);

            lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing );

            //lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0);       // 30.05.2014 OG: my_itoa() gibt es nicht mehr

            lcd_printp_at (8, 5, PSTR("\x1e"), 0);

            lcd_printp_at (9, 5, PSTR("Dist:"), 0);

            lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance );

            //lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0);     // 30.05.2014 OG: my_itoa() gibt es nicht mehr

            lcd_printp_at (20, 5, PSTR("m"), 0);

        }