Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
962 | - | 1 | |
2 | /****************************************************************/ |
||
3 | /* */ |
||
4 | /* NG-Video 5,8GHz */ |
||
5 | /* */ |
||
6 | /* Copyright (C) 2011 - gebad */ |
||
7 | /* */ |
||
8 | /* This code is distributed under the GNU Public License */ |
||
9 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
10 | /* */ |
||
11 | /****************************************************************/ |
||
12 | |||
13 | #include "usart.h" |
||
14 | #include "math.h" |
||
15 | #include <stdio.h> |
||
16 | |||
17 | #include "servo.h" |
||
18 | |||
19 | #define DLEFT 0 |
||
20 | #define DRIGHT 1 |
||
21 | #define DEG_TO_RAD 0.0174533// degrees to radians = PI / 180 |
||
22 | #define RAD_TO_DEG 57.2957795// radians to degrees = 180 / PI |
||
23 | |||
24 | |||
25 | typedef struct { |
||
26 | double distance; |
||
27 | double bearing; |
||
28 | } geo_t; |
||
29 | |||
30 | /**************************************************************/ |
||
31 | /* */ |
||
32 | /* RSSI Tracking */ |
||
33 | /* */ |
||
34 | /**************************************************************/ |
||
35 | |||
36 | void Tracking_RSSI(void) |
||
37 | { uint16_t u0, u1; |
||
38 | static uint8_t direction; |
||
39 | static uint16_t servo1pos; |
||
40 | |||
41 | u0 = (ADC_Read(RSSI0) * (uint32_t)udbm_korr_1)/UDBM_KORR_FA; |
||
42 | u1 = (ADC_Read(RSSI1) * (uint32_t)udbm_korr_2)/UDBM_KORR_FA; |
||
43 | |||
44 | if (direction == DLEFT) { |
||
45 | if (u0 < u1) { |
||
46 | if ( servo1pos > 0) --servo1pos; // Servo Endposition? |
||
47 | servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen |
||
48 | } |
||
49 | else |
||
50 | if (u0 > (u1 + track_hyst)) direction = DRIGHT; // Richtungwechsel, wenn Hysterese überschritten |
||
51 | } |
||
52 | else { |
||
53 | if (u0 > u1) { |
||
54 | if ( servo1pos < ServoSteps()) ++servo1pos; // Servo Endposition? |
||
55 | servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen |
||
56 | } |
||
57 | else |
||
58 | if ((track_hyst + u0) < u1) direction = DLEFT; // Richtungwechsel, wenn Hysterese überschritten |
||
59 | } |
||
60 | } |
||
61 | |||
62 | /**************************************************************/ |
||
63 | /* */ |
||
64 | /* GPS Tracking */ |
||
65 | /* */ |
||
66 | /**************************************************************/ |
||
67 | |||
68 | // Datenempfang vom MK ==> blinkt Antennensymbol |
||
69 | void Displ_wiRX(void) |
||
70 | { static uint8_t timer = 0; |
||
71 | char Sats; |
||
72 | |||
73 | if ((bat_low != 0) && (pmenu[0] == '\0')) { // nicht im Menü und nicht bei Akku leer blinken |
||
74 | lcdGotoXY(1, 0); |
||
75 | if (wi232RX) { // Datensatz vom MK empfangen? |
||
76 | if (timer < BLINK_PERIOD/2) |
||
77 | lcdPutc(1); |
||
78 | else { |
||
79 | Sats = navi_data.SatsInUse; // aktuell empfangene Satellitenanzahl |
||
80 | if (navi_data.SatsInUse > 9) |
||
81 | Sats = 'X'; // Anzeige einstellig, da kein Platz auf Display |
||
82 | else |
||
83 | Sats = navi_data.SatsInUse + '0'; // Umrechnung Char |
||
84 | lcdPutc(Sats); |
||
85 | } |
||
86 | } |
||
87 | else |
||
88 | lcdPutc(' '); |
||
89 | wi232RX = 0; |
||
90 | if (++timer == BLINK_PERIOD) timer = 0; |
||
91 | } |
||
92 | } |
||
93 | |||
94 | // Berechnung von Distanz und Winkels aus GPS-Daten home(MK eingeschaltet) |
||
95 | // zur aktuellen Position(nach Motorstart) |
||
96 | geo_t calc_geo(HomePos_t home, GPS_Pos_t pos) |
||
97 | { double lat1, lon1, lat2, lon2, d1, dlat; |
||
98 | geo_t geo; |
||
99 | |||
100 | lon1 = home.Home_Lon; |
||
101 | lat1 = home.Home_Lat; |
||
102 | lon2 = (double)pos.Longitude / 10000000; |
||
103 | lat2 = (double)pos.Latitude / 10000000; |
||
104 | |||
105 | // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
||
106 | // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
||
107 | // es wird jedoch in Meter weiter gerechnet |
||
108 | d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
||
109 | dlat = 111300 * (double)(lat1 - lat2); |
||
110 | // returns a value in metres http://www.kompf.de/gps/distcalc.html |
||
111 | geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
||
112 | if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
||
113 | geo.distance = sqrt(d1 * d1 + dlat * dlat); |
||
114 | return(geo); |
||
115 | } |
||
116 | |||
117 | // MK OSD-Daten lesen und verifizieren |
||
118 | uint8_t OSD_Data_valid(void) |
||
119 | { uint8_t ret = 0; |
||
120 | char *tx_osd = {"#co?]==EH\r"}; |
||
121 | // char interval[2] = {10, '\0'}; |
||
122 | |||
123 | if (rx_line_decode('O')) { // OSD-Datensatz prüfen/dekodieren |
||
124 | navi_data = *((NaviData_t*)data_decode); // dekodierte Daten mit Struktur OSD-Daten versehen |
||
125 | // GPS-Daten nicht zu alt und ok. |
||
126 | if ((rx_timeout < RX_TIME_OLD) && (navi_data.NCFlags & NC_FLAG_GPS_OK)) { |
||
127 | ret = 1; |
||
128 | } |
||
129 | } |
||
130 | // ca. 150ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
||
131 | // if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 10 * 10ms interval |
||
132 | if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 10 * 10ms interval |
||
133 | return(ret); |
||
134 | } |
||
135 | |||
136 | // Umrechnung Winkel in Servoschritte |
||
137 | void servo_track(uint8_t servo_nr, int16_t Angel) |
||
138 | { |
||
139 | servoSetPosition(servo_nr, (uint16_t)((float)Angel * ServoSteps() / 180)); |
||
140 | } |
||
141 | |||
142 | // MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
||
143 | // Hauptprogramm von GPS Antennen-Nachführung |
||
144 | void Tracking_GPS(void) |
||
145 | { geo_t geo; |
||
146 | int AngelPan, AngelTilt; |
||
147 | char puffer[30]; |
||
148 | |||
149 | if (OSD_Data_valid()) { |
||
150 | if (coldstart) { |
||
151 | // erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
||
152 | if (navi_data.FCStatusFlags & FC_FLAG_MOTOR_START) { |
||
153 | MK_pos.Home_Lon = (double)navi_data.HomePosition.Longitude / 10000000; |
||
154 | MK_pos.Home_Lat = (double)navi_data.HomePosition.Latitude / 10000000; |
||
155 | MK_pos.Home_Alt = navi_data.HomePosition.Altitude; |
||
156 | MK_pos.direction = navi_data.CompassHeading; |
||
157 | coldstart = 0; |
||
158 | } |
||
159 | } |
||
160 | else { |
||
161 | geo = calc_geo(MK_pos, navi_data.CurrentPosition); |
||
162 | AngelTilt = RAD_TO_DEG * (double)atan2((double)(navi_data.CurrentPosition.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
||
163 | if (geo.distance < 4) { // Abstand MK- zu Antennenposition < 4m ==> Pan-Servo in Mittelstellung |
||
164 | geo.bearing = MK_pos.direction; |
||
165 | AngelTilt = 0; |
||
166 | } |
||
167 | // egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
||
168 | // 360 grd negative Winkelwerte als positive |
||
169 | AngelPan = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360); |
||
170 | if (AngelPan > 360) AngelPan -= 360; |
||
171 | |||
172 | if (AngelTilt < 0) AngelTilt = 0; |
||
173 | if (AngelTilt > 180) AngelTilt = 180; |
||
174 | |||
175 | if (AngelPan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
||
176 | AngelPan = AngelPan - 180; |
||
177 | AngelTilt = 180 - AngelTilt; |
||
178 | } |
||
179 | |||
180 | servo_track(0, AngelPan); |
||
181 | servo_track(1, AngelTilt); |
||
182 | |||
183 | if ((gps_display > GPS_DISP_NONE) && (bat_low != 0)) { |
||
184 | if (gps_disp_clear) { |
||
185 | gps_disp_clear = 0; |
||
186 | lcdClear(); |
||
187 | } |
||
188 | // der Einfachheit halber doch noch mit Kommazahlen sprintf(...) |
||
189 | switch(gps_display) { |
||
190 | case GPS_DISP_CALC: |
||
191 | sprintf( puffer, "Dir:%3d", MK_pos.direction); |
||
192 | lcdGotoXY(0, 0); |
||
193 | lcdPuts(puffer); |
||
194 | sprintf( puffer, "Dis:%4.1f", geo.distance); |
||
195 | lcdGotoXY(8, 0); |
||
196 | lcdPuts(puffer); |
||
197 | sprintf( puffer, "Bea:%5.1f", geo.bearing); |
||
198 | lcdGotoXY(0, 1); |
||
199 | lcdPuts(puffer); |
||
200 | sprintf( puffer, "Pan:%3d", AngelPan); |
||
201 | lcdGotoXY(0, 2); |
||
202 | lcdPuts(puffer); |
||
203 | sprintf( puffer, "Til:%3d", AngelTilt); |
||
204 | lcdGotoXY(8, 2); |
||
205 | lcdPuts(puffer); |
||
206 | break; |
||
207 | case GPS_DISP_CURRENT: |
||
208 | sprintf( puffer, "aLon:%10.6f", (double)navi_data.CurrentPosition.Longitude / 10000000); |
||
209 | lcdGotoXY(0, 0); |
||
210 | lcdPuts(puffer); |
||
211 | sprintf( puffer, "aLat:%10.6f", (double)navi_data.CurrentPosition.Latitude / 10000000); |
||
212 | lcdGotoXY(0, 1); |
||
213 | lcdPuts(puffer); |
||
214 | sprintf( puffer, "aAlt:%10.1f", (double)navi_data.CurrentPosition.Altitude / 1000); |
||
215 | lcdGotoXY(0, 2); |
||
216 | lcdPuts(puffer); |
||
217 | break; |
||
218 | case GPS_DISP_HOME: |
||
219 | sprintf( puffer, "hLon:%10.6f", MK_pos.Home_Lon); |
||
220 | lcdGotoXY(0, 0); |
||
221 | lcdPuts(puffer); |
||
222 | sprintf( puffer, "hLat:%10.6f", MK_pos.Home_Lat); |
||
223 | lcdGotoXY(0, 1); |
||
224 | lcdPuts(puffer); |
||
225 | sprintf( puffer, "hAlt:%10.1f", (double)MK_pos.Home_Alt / 1000); |
||
226 | lcdGotoXY(0, 2); |
||
227 | lcdPuts(puffer); |
||
228 | break; |
||
229 | } |
||
230 | } |
||
231 | } |
||
232 | } |
||
233 | Displ_wiRX(); |
||
234 | } |
||
235 | |||
236 | /************************************************************************/ |
||
237 | /* */ |
||
238 | /* MKCockPit Tracking */ |
||
239 | /* */ |
||
240 | /* http://www.pololu.com/file/download/ssc03a_guide.pdf?file_id=0J37 * */ |
||
241 | /* */ |
||
242 | /************************************************************************/ |
||
243 | |||
244 | void Tracking_MKCockpit(void) |
||
245 | { uint16_t ServoPos; |
||
246 | |||
247 | if (Get_Pololu_cmd(data_decode, POLOLU_CMD)) { |
||
248 | switch(data_decode[0]) { |
||
249 | // Command 0: Set Parameters(1 data byte) |
||
250 | case 0x00: ; |
||
251 | break; |
||
252 | // Command 1: Set Speed (1 data byte) |
||
253 | case 0x01: ; |
||
254 | break; |
||
255 | // Command 4: Set Position,Absollute(2 data bytes) - angepasst für default MKCockPit |
||
256 | case 0x04:ServoPos = data_decode[2]; |
||
257 | ServoPos = ((ServoPos << 7) | data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu groß |
||
258 | |||
259 | if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0; |
||
260 | if (ServoPos > 180) ServoPos = 180; |
||
261 | servoSetPosition(data_decode[1], (uint16_t)((float)ServoPos * ServoSteps() / 180)); |
||
262 | } |
||
263 | } |
||
264 | } |