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