Subversion Repositories Projects

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//############################################################################

//# HISTORY  gps.c

//#

//# 20.09.2015 Startet

//# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen

//#    followme_calculate_offset(...)

//#

//# 03.08.2015 cebra

//# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen

//#    int nmea_move_horz(

//#    const nmeaPOS *start_pos,   /**< Start position in radians */

//#    nmeaPOS *end_pos,           /**< Result position in radians */

//#    double azimuth,             /**< Azimuth (degree) [0, 359] */

//#    double distance)             /**< Distance (km) */

//#

//# 27.06.2014 OG - NEU

//# - chg: auf #include "../gps/mymath.h" angepasst

//#

//# 20.06.2014 OG - NEU

//############################################################################

#include "../cpu.h"

#include <string.h>

#include <util/delay.h>

#include <avr/interrupt.h>

#include <stdlib.h>

#include <math.h>

#include "../main.h"

#include "../mk-data-structs.h"

#include "../gps/mymath.h"

#include "gps.h"

/*

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

*/

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

#define NMEA_PI                     (3.141592653589793)             /**< PI value */

#define NMEA_PI180                  (NMEA_PI / 180)                 /**< PI division by 180 */

#define NMEA_EARTHRADIUS_KM         (6378)                          /**< Earth's mean radius in km */

#define R                           (6371)

#define NMEA_EARTHRADIUS_M          (NMEA_EARTHRADIUS_KM * 1000)    /**< Earth's mean radius in m */

#define NMEA_EARTH_SEMIMAJORAXIS_M  (6378137.0)                     /**< Earth's semi-major axis in m according WGS84 */

#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */

#define NMEA_EARTH_FLATTENING       (1 / 298.257223563)             /**< Earth's flattening according WGS 84 */

#define NMEA_DOP_FACTOR             (5)                             /**< Factor for translating DOP to meters */

// Definitonen für FollowMeStep2

#define LONG_DIV                    10000000

#define LAT_DIV                     LONG_DIV

#define FOLLOWME_M2DEG              111111

#define FOLLOWME_ROUND_100          100

# define NMEA_POSIX(x)  x

/**

 * \fn nmea_degree2radian

 * \brief Convert degree to radian

 */

double nmea_degree2radian(double val)

{ return (val * NMEA_PI180); }

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

nmeaPOS NMEApos;

nmeaPOS NMEATarget;

/**

 * \brief Horizontal move of point position

 */

int nmea_move_horz(

    const nmeaPOS *start_pos,   /**< Start position in radians */

    nmeaPOS *end_pos,           /**< Result position in radians */

    double azimuth,             /**< Azimuth (degree) [0, 359] */

    double distance             /**< Distance (km) */

    )

{

    nmeaPOS p1 = *start_pos;

    int RetVal = 1;

    distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */

    azimuth = nmea_degree2radian(azimuth);

    end_pos->lat = asin( sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));

    end_pos->lon = p1.lon + atan2( sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));

    if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))

    {

        end_pos->lat = 0; end_pos->lon = 0;

        RetVal = 0;

    }

    return RetVal;

}

// Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück

//

// Benutzt die c_cos_8192 der FC

// TODO: move to followme.c

// TODO: *8192 optimieren

uint8_t followme_add_offset(

    const nmeaPOS *pPktPos,                     /**< Start position in radians */

    nmeaPOS *target_pos,                        /**< Result position in radians */

        positionOffset *pFollowMeOffset         /**< Position Offset in Millimeters */

    )

{

        nmeaPOS pktPos = *pPktPos;

        positionOffset followMeOffset = * pFollowMeOffset;

    target_pos->lat = pktPos.lat + ( ( followMeOffset.offset_lat * ( LAT_DIV  / FOLLOWME_M2DEG ) ) ) / 1000;

    target_pos->lon = pktPos.lon + ( ( followMeOffset.offset_long * ( LONG_DIV  / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.lat / LONG_DIV ) ) ) );

        return 1;

}

int16_t my_abs(int16_t input)

{

        if(input < 0)

                return -input;

        else

                return input;

}

// Rechnet einen Offset aus Radius und Winkel nach Lat/Long

// Benutzt die c_cos_8192 und c_sin_8192 der FC

// TODO: move to followme.c

uint8_t followme_calculate_offset(

                int32_t radius,                         // in mm

                int16_t angle,                          // in Grad °

                positionOffset *followMeOffset

                )

{

        angle %= 360;                   // map angle to 0° - 360°

        followMeOffset->offset_lat = ( radius * c_cos_8192( angle ) ) / 8192;

        followMeOffset->offset_long = ( radius * c_sin_8192( angle ) ) / 8192;

        return 1;

}

//###############################################################################################

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

GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )

{

    int32_t      lat1, lon1, lat2, lon2;

    int32_t      d1, dlat;

    GPS_PosDev_t PosDev;

    lon1 = pos1.Longitude;

    lat1 = pos1.Latitude;

    lon2 = pos2.Longitude;

    lat2 = pos2.Latitude;

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

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

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

    PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat );          //

    //PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10;       // *10 um von dm auf cm zu kommen

    return PosDev;

}

///**

// * \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);

        }

        return(geo);

}

*/