Subversion Repositories Projects

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

//############################################################################
//# HISTORY  gps.c
//#
//#
//#
//#
//#
//#
//#
//#
//# 22.09.2015 Starter
//# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT
//# - add my_abs(...)
//#
//# 20.09.2015 Starter
//# - 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->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth));

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

    if(NMEA_POSIX(isnan)(end_pos->latitude) || NMEA_POSIX(isnan)(end_pos->longitude))
    {
        end_pos->latitude = 0; end_pos->longitude = 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->latitude = pktPos.latitude + ( ( followMeOffset.latitude * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000;
    target_pos->longitude = pktPos.longitude + ( ( followMeOffset.longitude * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.latitude / LONG_DIV ) ) ) );
        return 1;
}


// schlanke abs-Methode
// TODO: move to mymath.h
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->latitude = ( radius * c_cos_8192( angle ) ) / 8192;
        followMeOffset->longitude = ( 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;
}