Rev 2200 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2136 | - | 1 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
3 | |||
4 | //############################################################################ |
||
5 | //# HISTORY gps.c |
||
6 | //# |
||
2212 | - | 7 | //# |
8 | //# |
||
9 | //# |
||
10 | //# |
||
11 | //# |
||
12 | //# |
||
13 | //# |
||
14 | //# 22.09.2015 Starter |
||
15 | //# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT |
||
16 | //# - add my_abs(...) |
||
17 | //# |
||
18 | //# 20.09.2015 Starter |
||
2200 | - | 19 | //# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
20 | //# followme_calculate_offset(...) |
||
21 | //# |
||
2136 | - | 22 | //# 03.08.2015 cebra |
23 | //# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
||
24 | //# int nmea_move_horz( |
||
25 | //# const nmeaPOS *start_pos, /**< Start position in radians */ |
||
26 | //# nmeaPOS *end_pos, /**< Result position in radians */ |
||
27 | //# double azimuth, /**< Azimuth (degree) [0, 359] */ |
||
28 | //# double distance) /**< Distance (km) */ |
||
29 | //# |
||
30 | //# 27.06.2014 OG - NEU |
||
31 | //# - chg: auf #include "../gps/mymath.h" angepasst |
||
32 | //# |
||
33 | //# 20.06.2014 OG - NEU |
||
34 | //############################################################################ |
||
35 | |||
36 | |||
37 | #include "../cpu.h" |
||
38 | #include <string.h> |
||
39 | #include <util/delay.h> |
||
40 | #include <avr/interrupt.h> |
||
41 | #include <stdlib.h> |
||
42 | #include <math.h> |
||
43 | #include "../main.h" |
||
44 | |||
45 | #include "../mk-data-structs.h" |
||
46 | #include "../gps/mymath.h" |
||
47 | #include "gps.h" |
||
48 | |||
49 | |||
50 | /* |
||
51 | // definiert in: mk_data-stucts.h |
||
52 | typedef struct |
||
53 | { |
||
54 | u16 Distance; // distance to target in cm |
||
55 | s16 Bearing; // course to target in deg |
||
56 | } __attribute__((packed)) GPS_PosDev_t; |
||
57 | */ |
||
58 | |||
59 | /* |
||
60 | // definiert in: mk_data-stucts.h |
||
61 | typedef struct |
||
62 | { |
||
63 | s32 Longitude; // in 1E-7 deg |
||
64 | s32 Latitude; // in 1E-7 deg |
||
65 | s32 Altitude; // in mm |
||
66 | u8 Status; // validity of data |
||
67 | } __attribute__((packed)) GPS_Pos_t; |
||
68 | */ |
||
69 | |||
70 | |||
71 | //-------------------------------------------------------------- |
||
72 | |||
73 | #define NMEA_PI (3.141592653589793) /**< PI value */ |
||
74 | #define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ |
||
75 | #define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ |
||
76 | #define R (6371) |
||
77 | #define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ |
||
78 | #define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ |
||
79 | #define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ |
||
80 | #define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
||
81 | #define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
||
82 | |||
2196 | - | 83 | |
84 | // Definitonen für FollowMeStep2 |
||
85 | #define LONG_DIV 10000000 |
||
86 | #define LAT_DIV LONG_DIV |
||
2199 | - | 87 | #define FOLLOWME_M2DEG 111111 |
88 | #define FOLLOWME_ROUND_100 100 |
||
2196 | - | 89 | |
90 | |||
2136 | - | 91 | # define NMEA_POSIX(x) x |
92 | |||
93 | |||
94 | |||
95 | /** |
||
96 | * \fn nmea_degree2radian |
||
97 | * \brief Convert degree to radian |
||
98 | */ |
||
99 | double nmea_degree2radian(double val) |
||
100 | { return (val * NMEA_PI180); } |
||
101 | |||
102 | |||
103 | //------------------------------------------------------------------------------------------ |
||
104 | nmeaPOS NMEApos; |
||
105 | nmeaPOS NMEATarget; |
||
106 | |||
107 | /** |
||
108 | * \brief Horizontal move of point position |
||
109 | */ |
||
110 | int nmea_move_horz( |
||
111 | const nmeaPOS *start_pos, /**< Start position in radians */ |
||
112 | nmeaPOS *end_pos, /**< Result position in radians */ |
||
113 | double azimuth, /**< Azimuth (degree) [0, 359] */ |
||
114 | double distance /**< Distance (km) */ |
||
115 | ) |
||
116 | { |
||
117 | nmeaPOS p1 = *start_pos; |
||
118 | int RetVal = 1; |
||
119 | |||
120 | distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
||
121 | azimuth = nmea_degree2radian(azimuth); |
||
122 | |||
2212 | - | 123 | end_pos->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth)); |
2136 | - | 124 | |
2212 | - | 125 | end_pos->longitude = p1.longitude + atan2( sin(azimuth) * sin(distance) * cos(p1.latitude), cos(distance) - sin(p1.latitude) * sin(end_pos->latitude)); |
2136 | - | 126 | |
2212 | - | 127 | if(NMEA_POSIX(isnan)(end_pos->latitude) || NMEA_POSIX(isnan)(end_pos->longitude)) |
2136 | - | 128 | { |
2212 | - | 129 | end_pos->latitude = 0; end_pos->longitude = 0; |
2136 | - | 130 | RetVal = 0; |
131 | } |
||
132 | |||
133 | return RetVal; |
||
134 | } |
||
135 | |||
136 | |||
2212 | - | 137 | |
138 | |||
139 | |||
140 | // Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück |
||
141 | // |
||
2200 | - | 142 | // Benutzt die c_cos_8192 der FC |
143 | // TODO: move to followme.c |
||
2212 | - | 144 | // TODO: *8192 optimieren |
2136 | - | 145 | |
2212 | - | 146 | uint8_t followme_add_offset( |
147 | const nmeaPOS *pPktPos, /**< Start position in radians */ |
||
148 | nmeaPOS *target_pos, /**< Result position in radians */ |
||
149 | positionOffset *pFollowMeOffset /**< Position Offset in Millimeters */ |
||
2196 | - | 150 | ) |
151 | { |
||
2200 | - | 152 | nmeaPOS pktPos = *pPktPos; |
2212 | - | 153 | positionOffset followMeOffset = * pFollowMeOffset; |
2200 | - | 154 | |
2212 | - | 155 | target_pos->latitude = pktPos.latitude + ( ( followMeOffset.latitude * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000; |
156 | target_pos->longitude = pktPos.longitude + ( ( followMeOffset.longitude * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.latitude / LONG_DIV ) ) ) ); |
||
157 | return 1; |
||
158 | } |
||
2196 | - | 159 | |
2212 | - | 160 | |
161 | // schlanke abs-Methode |
||
162 | // TODO: move to mymath.h |
||
163 | int16_t my_abs(int16_t input) |
||
164 | { |
||
165 | if(input < 0) |
||
166 | return -input; |
||
167 | else |
||
168 | return input; |
||
169 | } |
||
170 | |||
171 | // Rechnet einen Offset aus Radius und Winkel nach Lat/Long |
||
172 | // Benutzt die c_cos_8192 und c_sin_8192 der FC |
||
173 | // TODO: move to followme.c |
||
174 | |||
175 | uint8_t followme_calculate_offset( |
||
176 | int32_t radius, // in mm |
||
177 | int16_t angle, // in Grad ° |
||
178 | positionOffset *followMeOffset |
||
179 | ) |
||
180 | { |
||
181 | angle %= 360; // map angle to 0° - 360° |
||
182 | |||
183 | followMeOffset->latitude = ( radius * c_cos_8192( angle ) ) / 8192; |
||
184 | followMeOffset->longitude = ( radius * c_sin_8192( angle ) ) / 8192; |
||
185 | |||
2196 | - | 186 | return 1; |
187 | } |
||
188 | |||
189 | |||
2136 | - | 190 | //############################################################################################### |
191 | |||
192 | |||
193 | |||
194 | //-------------------------------------------------------------- |
||
195 | GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
||
196 | { |
||
197 | int32_t lat1, lon1, lat2, lon2; |
||
198 | int32_t d1, dlat; |
||
199 | GPS_PosDev_t PosDev; |
||
200 | |||
201 | lon1 = pos1.Longitude; |
||
202 | lat1 = pos1.Latitude; |
||
203 | |||
204 | lon2 = pos2.Longitude; |
||
205 | lat2 = pos2.Latitude; |
||
206 | |||
207 | d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
||
208 | dlat = (1113 * (lat1 - lat2) / 10000); |
||
209 | |||
210 | PosDev.Bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
||
211 | PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ); // |
||
212 | //PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10; // *10 um von dm auf cm zu kommen |
||
213 | |||
214 | return PosDev; |
||
215 | } |
||
216 | |||
217 | |||
218 |