Rev 839 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
838 | MikeW | 1 | /* |
2 | Copyright 2008, by Michael Walter |
||
3 | |||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
||
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
||
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
||
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
9 | |||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
||
12 | unless it is stated otherwise. |
||
13 | */ |
||
14 | |||
15 | #include "main.h" |
||
16 | #include "kafi.h" |
||
966 | MikeW | 17 | #include "mymath.h" |
838 | MikeW | 18 | |
966 | MikeW | 19 | #define sin45 -0.707106 |
20 | #define cos45 0.707106 |
||
21 | |||
838 | MikeW | 22 | extern void UART2Print(void); |
23 | void InitOSD(void); |
||
24 | void SendOSD(void); |
||
25 | |||
966 | MikeW | 26 | extern gpsInfo_t actualPos;// measured position (last gps record) |
27 | extern gpsInfo_t targetPos;// measured position (last gps record) |
||
838 | MikeW | 28 | |
29 | extern signed int GPS_Nick; |
||
30 | extern signed int GPS_Roll; |
||
31 | |||
32 | extern int CurrentAltitude, LastAltitude, targetPosValid, RCQuality; |
||
966 | MikeW | 33 | extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d; |
838 | MikeW | 34 | extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45; |
966 | MikeW | 35 | extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P; |
838 | MikeW | 36 | extern unsigned long maxDistance; |
37 | |||
966 | MikeW | 38 | int Theta45; |
39 | int Phi45; |
||
40 | |||
838 | MikeW | 41 | unsigned char UART2PrintAbgeschlossen = 1; |
42 | void UART2Print(void); |
||
43 | |||
44 | char DisplayBuff[80] = ""; |
||
45 | unsigned char DispPtr = 0; |
||
46 | |||
47 | char OSDBuff[80] = ""; |
||
48 | unsigned char OSDPtr = 0; |
||
49 | |||
50 | |||
51 | #define OSD_printf(format, args...) { OSDPtr = 0; _printf_P(OUT_OSD,PSTR(format) , ## args);UART2Print();} |
||
52 | #define OSD_printf_(format, args...) { _printf_P(OUT_OSD,PSTR(format) , ## args);} |
||
53 | |||
54 | |||
55 | /* For OSD_printf to work, Putchar() has to be extendet as follows. |
||
56 | char Putchar(char zeichen) |
||
57 | { |
||
966 | MikeW | 58 | if(PrintZiel == OUT_LCD) |
59 | { |
||
60 | DisplayBuff[DispPtr++] = zeichen; return(1); |
||
838 | MikeW | 61 | } |
966 | MikeW | 62 | else if (PrintZiel == OUT_OSD) |
63 | { |
||
64 | OSDBuff[OSDPtr++] = zeichen; return(1); |
||
65 | } |
||
66 | else |
||
67 | { |
||
68 | return(uart_putchar(zeichen)); |
||
69 | } |
||
70 | } |
||
838 | MikeW | 71 | */ |
72 | |||
73 | |||
74 | /* **************************************************************************** |
||
75 | Functionname: OsdClear */ /*! |
||
76 | Description: |
||
77 | |||
78 | @param[in] |
||
79 | |||
80 | @return void |
||
81 | @pre - |
||
82 | @post - |
||
83 | @author Michael Walter |
||
84 | **************************************************************************** */ |
||
85 | void OsdClear(void) |
||
86 | { |
||
966 | MikeW | 87 | unsigned char i; |
88 | for(i=0;i<80;i++) OSDBuff[i] = ' '; |
||
838 | MikeW | 89 | } |
90 | |||
91 | /* **************************************************************************** |
||
92 | Functionname: LcdClear */ /*! |
||
93 | Description: |
||
94 | |||
95 | @param[in] |
||
96 | |||
97 | @return void |
||
98 | @pre - |
||
99 | @post - |
||
100 | @author |
||
101 | **************************************************************************** */ |
||
102 | void LcdClear(void) |
||
103 | { |
||
966 | MikeW | 104 | unsigned char i; |
105 | for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
||
838 | MikeW | 106 | } |
107 | |||
108 | |||
109 | /* **************************************************************************** |
||
110 | Functionname: InitOSD */ /*! |
||
111 | Description: Init the Bob-4H OSD |
||
112 | |||
113 | @param[in] |
||
114 | |||
115 | @return void |
||
116 | @pre - |
||
117 | @post - |
||
118 | @author Michael Walter |
||
119 | **************************************************************************** */ |
||
120 | void InitOSD() |
||
121 | { |
||
966 | MikeW | 122 | /* Clear OSD */ |
123 | OSD_printf ("\33[2J"); |
||
124 | Delay_ms_Mess(300); |
||
125 | OSD_printf ("\33[2J"); |
||
126 | |||
127 | /* Set OSD Pixel Clock */ |
||
128 | Delay_ms_Mess(300); |
||
129 | OSD_printf ("\33[23;6v"); |
||
130 | |||
131 | /* Set OSD Pixel Width */ |
||
132 | Delay_ms_Mess(300); |
||
133 | OSD_printf ("\33[25;448v"); |
||
134 | |||
135 | /* Set OSD Font */ |
||
136 | Delay_ms_Mess(300); |
||
137 | OSD_printf ("\33[0z"); |
||
838 | MikeW | 138 | } |
139 | |||
140 | |||
141 | |||
142 | /* **************************************************************************** |
||
143 | Functionname: UART2Print */ /*! |
||
144 | Description: Send OSD Data to the Bob-4H OSD |
||
145 | |||
146 | @return void |
||
147 | @pre - |
||
148 | @post - |
||
149 | @author Michael Walter |
||
150 | **************************************************************************** */ |
||
151 | void UART2Print() |
||
152 | { |
||
966 | MikeW | 153 | OSDBuff[OSDPtr] = '\r'; |
154 | if (UART2PrintAbgeschlossen == 1) |
||
155 | { |
||
156 | UART2PrintAbgeschlossen = 0; |
||
157 | UDR1 = OSDBuff[0]; |
||
158 | } |
||
838 | MikeW | 159 | } |
160 | |||
161 | /* **************************************************************************** |
||
162 | Functionname: SIGNAL */ /*! |
||
163 | Description: Send OSD Data to the Bob-4H OSD |
||
164 | |||
165 | @return void |
||
166 | @pre - |
||
167 | @post - |
||
168 | @author Michael Walter |
||
169 | **************************************************************************** */ |
||
170 | SIGNAL(SIG_USART1_TRANS) |
||
171 | { |
||
966 | MikeW | 172 | static unsigned int ptr = 0; |
173 | |||
174 | unsigned char tmp_tx; |
||
175 | if(!UART2PrintAbgeschlossen) |
||
176 | { |
||
177 | ptr++; // die [0] wurde schon gesendet |
||
178 | tmp_tx = OSDBuff[ptr]; |
||
179 | if((tmp_tx == '\r') /* tmp_tx == 0 */) |
||
180 | { |
||
181 | ptr = 0; |
||
182 | UART2PrintAbgeschlossen = 1; |
||
183 | } |
||
184 | UDR1 = tmp_tx; |
||
185 | } |
||
186 | else ptr = 0; |
||
838 | MikeW | 187 | } |
188 | |||
189 | |||
190 | /* **************************************************************************** |
||
191 | Functionname: SendOSD */ /*! |
||
192 | Description: Display OnScreen Display Data on a Bob-4H |
||
193 | |||
194 | @param[in] |
||
195 | |||
196 | @return void |
||
197 | @pre - |
||
198 | @post - |
||
199 | @author Michael Walter |
||
200 | **************************************************************************** */ |
||
201 | void SendOSD() |
||
202 | { |
||
966 | MikeW | 203 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
204 | |||
205 | /*--- VARIABLES ---*/ |
||
206 | static int dx, dy, x1,y1,x2,y2; |
||
207 | f32_t sinPhi_P_cosTheta_P; |
||
208 | static int x1_old, y1_old, x2_old, y2_old; |
||
209 | static int iState = 0; |
||
210 | |||
211 | switch (iState) |
||
212 | { |
||
213 | case 0: |
||
214 | OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10); |
||
215 | iState++; |
||
216 | break; |
||
217 | case 1: /* Display the battery voltage an the RC signal level */ |
||
218 | OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality); |
||
219 | iState++; |
||
220 | break; |
||
221 | case 2: /* Display the current altitude */ |
||
222 | if (targetPosValid == 1) |
||
223 | { |
||
224 | int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude); |
||
225 | OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude); |
||
226 | } |
||
227 | else |
||
228 | { |
||
229 | OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude); |
||
230 | } |
||
231 | iState++; |
||
232 | break; |
||
233 | case 3: /* Draw an artificial horizon. Part 1 */ |
||
234 | #if 1 |
||
235 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
||
236 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
||
237 | Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100); |
||
238 | dx = c_cos_8192(Phi45) / 128; |
||
239 | dy = c_sin_8192(Phi45) / 128; |
||
240 | x1 = 180 - dx; |
||
241 | y1 = 120 + 2 * Theta45 + dy; |
||
242 | x2 = 180 + dx; |
||
243 | y2 = y1 - 2 * dy; |
||
244 | |||
245 | OSDPtr = 0; |
||
246 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
||
838 | MikeW | 247 | #endif |
966 | MikeW | 248 | iState++; |
249 | break; |
||
250 | case 4: /* Draw an artificial horizon. Part 2 */ |
||
251 | #if 1 |
||
252 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
||
253 | UART2Print(); |
||
254 | x1_old = x1; |
||
255 | y1_old = y1; |
||
256 | x2_old = x2; |
||
257 | y2_old = y2; |
||
258 | #endif |
||
259 | iState++; |
||
260 | break; |
||
261 | case 5: /* Display velocity over ground */ |
||
262 | if (actualPos.state > 1) |
||
263 | { |
||
264 | OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100); |
||
265 | } |
||
266 | else |
||
267 | { |
||
268 | OSD_printf ("\33[0;20HVel:-.-"); |
||
269 | } |
||
270 | iState++; |
||
271 | break; |
||
272 | case 6: /* Display distance from target */ |
||
273 | if (targetPosValid == 1) |
||
274 | { |
||
275 | OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10); |
||
276 | } |
||
277 | else |
||
278 | { |
||
279 | OSD_printf ("\33[1;20HDst:-.-"); |
||
280 | } |
||
281 | iState = 8; |
||
282 | break; |
||
283 | case 7: |
||
284 | OSD_printf ("\33[4;0HN:%03d\33[5;0HR:%03d", status.iTheta10 / 10, status.iPhi10 / 10); |
||
285 | iState++; |
||
286 | break; |
||
287 | case 8: /* Draw an artificial horizon. Part 1 */ |
||
288 | #if 1 |
||
289 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
||
290 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
||
291 | Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100); |
||
292 | dx = c_cos_8192(Phi45) / 128; |
||
293 | dy = c_sin_8192(Phi45) / 128; |
||
294 | x1 = 180 - dx; |
||
295 | y1 = 120 + 2 * Theta45 + dy; |
||
296 | x2 = 180 + dx; |
||
297 | y2 = y1 - 2 * dy; |
||
298 | |||
299 | OSDPtr = 0; |
||
300 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
||
301 | #endif |
||
302 | iState++; |
||
303 | break; |
||
304 | case 9: /* Draw an artificial horizon. Part 2 */ |
||
305 | #if 1 |
||
306 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
||
307 | UART2Print(); |
||
308 | |||
309 | x1_old = x1; |
||
310 | y1_old = y1; |
||
311 | x2_old = x2; |
||
312 | y2_old = y2; |
||
313 | #endif |
||
314 | iState++; |
||
315 | break; |
||
316 | case 10: /* Display the GPS control outputs */ |
||
317 | OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p, roll_gain_p, nick_gain_d, roll_gain_d); |
||
318 | iState++; |
||
319 | break; |
||
320 | case 11: |
||
321 | OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude); |
||
322 | iState++; |
||
323 | break; |
||
324 | case 12:/* Draw a ^ to indicate the target direction */ |
||
325 | { |
||
326 | static int LastGierOffset = 0; |
||
327 | int GierOffset = (TargetGier - status.iPsi10) / 10; |
||
328 | if (GierOffset > 180) |
||
329 | { |
||
330 | GierOffset -= 360; |
||
331 | } |
||
332 | if (GierOffset < -180) |
||
333 | { |
||
334 | GierOffset += 360; |
||
335 | } |
||
336 | GierOffset /= 14; |
||
337 | |||
338 | OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset); |
||
339 | LastGierOffset = GierOffset; |
||
340 | iState = 14; |
||
341 | break; |
||
342 | } |
||
343 | case 13: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */ |
||
344 | if (Override == 0) |
||
345 | { |
||
346 | OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll); |
||
347 | } |
||
348 | else |
||
349 | { |
||
350 | OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll); |
||
351 | } |
||
352 | iState++; |
||
353 | break; |
||
354 | case 14: |
||
355 | #if 1/* Display the Horizon */ |
||
356 | OSD_printf ("\33[156;120.r\33[204;120+r\33[/r "); |
||
838 | MikeW | 357 | #endif |
966 | MikeW | 358 | iState=0; |
359 | break; |
||
360 | default: |
||
361 | iState = 0; |
||
362 | } |
||
838 | MikeW | 363 | } |
364 |