Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2136 - 1
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3
 
4
//############################################################################
5
//# HISTORY  gps.c
6
//#
7
//# 03.08.2015 cebra
8
//# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen
9
//#    int nmea_move_horz(
10
//#    const nmeaPOS *start_pos,   /**< Start position in radians */
11
//#    nmeaPOS *end_pos,           /**< Result position in radians */
12
//#    double azimuth,             /**< Azimuth (degree) [0, 359] */
13
//#    double distance)             /**< Distance (km) */
14
//#
15
//# 27.06.2014 OG - NEU
16
//# - chg: auf #include "../gps/mymath.h" angepasst
17
//#
18
//# 20.06.2014 OG - NEU
19
//############################################################################
20
 
21
 
22
#include "../cpu.h"
23
#include <string.h>
24
#include <util/delay.h>
25
#include <avr/interrupt.h>
26
#include <stdlib.h>
27
#include <math.h>
28
#include "../main.h"
29
 
30
#include "../mk-data-structs.h"
31
#include "../gps/mymath.h"
32
#include "gps.h"
33
 
34
 
35
/*
36
// definiert in: mk_data-stucts.h
37
typedef struct
38
{
39
    u16 Distance;       // distance to target in cm
40
    s16 Bearing;        // course to target in deg
41
} __attribute__((packed)) GPS_PosDev_t;
42
*/
43
 
44
/*
45
// definiert in: mk_data-stucts.h
46
typedef struct
47
{
48
    s32 Longitude;      // in 1E-7 deg
49
    s32 Latitude;       // in 1E-7 deg
50
    s32 Altitude;       // in mm
51
    u8 Status;          // validity of data
52
} __attribute__((packed)) GPS_Pos_t;
53
*/
54
 
55
 
56
//--------------------------------------------------------------
57
 
58
#define NMEA_PI                     (3.141592653589793)             /**< PI value */
59
#define NMEA_PI180                  (NMEA_PI / 180)                 /**< PI division by 180 */
60
#define NMEA_EARTHRADIUS_KM         (6378)                          /**< Earth's mean radius in km */
61
#define R                           (6371)
62
#define NMEA_EARTHRADIUS_M          (NMEA_EARTHRADIUS_KM * 1000)    /**< Earth's mean radius in m */
63
#define NMEA_EARTH_SEMIMAJORAXIS_M  (6378137.0)                     /**< Earth's semi-major axis in m according WGS84 */
64
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */
65
#define NMEA_EARTH_FLATTENING       (1 / 298.257223563)             /**< Earth's flattening according WGS 84 */
66
#define NMEA_DOP_FACTOR             (5)                             /**< Factor for translating DOP to meters */
67
 
68
# define NMEA_POSIX(x)  x
69
 
70
 
71
 
72
/**
73
 * \fn nmea_degree2radian
74
 * \brief Convert degree to radian
75
 */
76
double nmea_degree2radian(double val)
77
{ return (val * NMEA_PI180); }
78
 
79
 
80
//------------------------------------------------------------------------------------------
81
nmeaPOS NMEApos;
82
nmeaPOS NMEATarget;
83
 
84
/**
85
 * \brief Horizontal move of point position
86
 */
87
int nmea_move_horz(
88
    const nmeaPOS *start_pos,   /**< Start position in radians */
89
    nmeaPOS *end_pos,           /**< Result position in radians */
90
    double azimuth,             /**< Azimuth (degree) [0, 359] */
91
    double distance             /**< Distance (km) */
92
    )
93
{
94
    nmeaPOS p1 = *start_pos;
95
    int RetVal = 1;
96
 
97
    distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
98
    azimuth = nmea_degree2radian(azimuth);
99
 
100
    end_pos->lat = asin( sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
101
 
102
    end_pos->lon = p1.lon + atan2( sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
103
 
104
    if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
105
    {
106
        end_pos->lat = 0; end_pos->lon = 0;
107
        RetVal = 0;
108
    }
109
 
110
    return RetVal;
111
}
112
 
113
 
114
 
115
//###############################################################################################
116
 
117
 
118
 
119
//--------------------------------------------------------------
120
GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )
121
{
122
    int32_t      lat1, lon1, lat2, lon2;
123
    int32_t      d1, dlat;
124
    GPS_PosDev_t PosDev;
125
 
126
    lon1 = pos1.Longitude;
127
    lat1 = pos1.Latitude;
128
 
129
    lon2 = pos2.Longitude;
130
    lat2 = pos2.Latitude;
131
 
132
    d1   = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
133
    dlat = (1113 * (lat1 - lat2) / 10000);
134
 
135
    PosDev.Bearing  = (my_atan2(d1, dlat) + 540) % 360;         // 360 +180 besserer Vergleich mit MkCockpit
136
    PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat );          //
137
    //PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10;       // *10 um von dm auf cm zu kommen
138
 
139
    return PosDev;
140
}
141
 
142
 
143
///**
144
// * \brief Calculate distance between two points
145
// * \return Distance in meters
146
// */
147
//int32_t nmea_distance(
148
//        const nmeaPOS *from_pos,    /**< From position in radians */
149
//        const nmeaPOS *to_pos       /**< To position in radians */
150
//        )
151
//{
152
//  int32_t dist = ((int32_t)NMEA_EARTHRADIUS_M) * acos(
153
//        sin(to_pos->lat) * sin(from_pos->lat) +
154
//        cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
155
//        );
156
//    return dist;
157
//}
158
 
159
 
160
 
161
//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
162
//// zur aktuellen Position(nach Motorstart)
163
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)
164
//{ double lat1, lon1, lat2, lon2, d1, dlat;
165
//        geo_t geo;
166
//
167
//        lon1 = MK_pos.Home_Lon;
168
//        lat1 = MK_pos.Home_Lat;
169
//        lon2 = (double)pos->Longitude   / 10000000.0;
170
//        lat2 = (double)pos->Latitude    / 10000000.0;
171
//
172
//        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
173
//        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator
174
//        // es wird jedoch in Meter weiter gerechnet
175
//        d1       = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
176
//        dlat = 111300 * (double)(lat1 - lat2);
177
//        // returns a value in metres http://www.kompf.de/gps/distcalc.html
178
//        geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
179
//        if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400
180
//        geo.distance = sqrt(d1 * d1 + dlat * dlat);
181
//        return(geo);
182
//}
183
 
184
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
185
// zur aktuellen Position(nach Motorstart)
186
//--------------------------------------------------------------
187
//--------------------------------------------------------------
188
 
189
/*
190
geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos )
191
{
192
    int32_t lat1, lon1, lat2, lon2;
193
        int32_t d1, dlat;
194
        geo_t geo;
195
 
196
        lon1 = home->Home_Lon;
197
        lat1 = home->Home_Lat;
198
        lon2 = pos->Longitude;
199
        lat2 = pos->Latitude;
200
 
201
        if( !CheckGPS )
202
        {
203
            writex_gpspos(  0, 3, home->Home_Lat , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)
204
            writex_gpspos( 11, 3, home->Home_Lon , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)
205
            writex_gpspos(  0, 4, pos->Latitude  , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)
206
            writex_gpspos( 11, 4, pos->Longitude , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)
207
 
208
            //lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0);     // 30.05.2014 OG: my_itoa() gibt es nicht mehr
209
            //lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr
210
            //lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0);      // 30.05.2014 OG: my_itoa() gibt es nicht mehr
211
            //lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr
212
        }
213
 
214
        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
215
        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator
216
        // es wird jedoch in dm Meter weiter gerechnet
217
        // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1
218
        d1       = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
219
        dlat = 1113 * (lat1 - lat2) / 10000;
220
        geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
221
        geo.distance = sqrt32(d1 * d1 + dlat * dlat);
222
        if( !CheckGPS )
223
        {
224
            lcd_printp_at (0, 5, PSTR("Bear:"), 0);
225
 
226
            lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing );
227
            //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
228
 
229
            lcd_printp_at (8, 5, PSTR("\x1e"), 0);
230
            lcd_printp_at (9, 5, PSTR("Dist:"), 0);
231
 
232
            lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance );
233
            //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
234
 
235
            lcd_printp_at (20, 5, PSTR("m"), 0);
236
        }
237
 
238
 
239
        return(geo);
240
}
241
*/
242