Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1702 - 1
/******* STARTUP PANEL *******/
2
 
3
void startPanels(){
4
  osd.clear();
5
  // Display our logo
6
  panLogo(10,5);
7
}
8
 
9
/******* PANELS - POSITION *******/
10
 
11
void writePanels(){
12
  if(millis() < (lastMAVBeat + 2000)){
13
    //osd.clear();
14
    //Testing bits from 8 bit register A
15
    if(ISa(Cen_BIT)) panCenter(panCenter_XY[0], panCenter_XY[1]);   //4x2
16
    if(ISa(Pit_BIT)) panPitch(panPitch_XY[0], panPitch_XY[1]); //5x1
17
    if(ISa(Rol_BIT)) panRoll(panRoll_XY[0], panRoll_XY[1]); //5x1
18
    if(ISa(BatA_BIT)) panBatt_A(panBatt_A_XY[0], panBatt_A_XY[1]); //7x1
19
//  if(ISa(BatB_BIT)) panBatt_B(panBatt_B_XY[0], panBatt_B_XY[1]); //7x1
20
    if(ISa(GPSats_BIT)) panGPSats(panGPSats_XY[0], panGPSats_XY[1]); //5x1
21
    if(ISa(GPL_BIT)) panGPL(panGPL_XY[0], panGPL_XY[1]); //2x1
22
    if(ISa(GPS_BIT)) panGPS(panGPS_XY[0], panGPS_XY[1]); //12x3
23
 
24
    //Testing bits from 8 bit register B
25
    if(ISb(Rose_BIT)) panRose(panRose_XY[0], panRose_XY[1]);        //13x3
26
    if(ISb(Head_BIT)) panHeading(panHeading_XY[0], panHeading_XY[1]); //13x3
27
    if(ISb(MavB_BIT)) panMavBeat(panMavBeat_XY[0], panMavBeat_XY[1]); //13x3
28
 
29
    if(osd_got_home == 1){
30
      if(ISb(HDis_BIT)) panHomeDis(panHomeDis_XY[0], panHomeDis_XY[1]); //13x3
31
      if(ISb(HDir_BIT)) panHomeDir(panHomeDir_XY[0], panHomeDir_XY[1]); //13x3
32
    }
33
//  if(ISb(WDir_BIT)) panWayPDir(panWayPDir_XY[0], panWayPDir_XY[1]); //??x??
34
//  if(ISb(WDis_BIT)) panWayPDis(panWayPDis_XY[0], panWayPDis_XY[1]); //??x??
35
//  if(ISb(WRSSI_BIT)) panRSSI(panRSSI_XY[0], panRSSI_XY[1]); //??x??
36
 
37
    //Testing bits from 8 bit register C
38
    //if(osd_got_home == 1){
39
      if(ISc(Alt_BIT)) panAlt(panAlt_XY[0], panAlt_XY[1]); //
40
      if(ISc(Vel_BIT)) panVel(panVel_XY[0], panVel_XY[1]); //
41
    //}
42
    if(ISc(Thr_BIT)) panThr(panThr_XY[0], panThr_XY[1]); //
43
    if(ISc(FMod_BIT)) panFlightMode(panFMod_XY[0], panFMod_XY[1]);  //
44
    if(ISc(Hor_BIT)) panHorizon(panHorizon_XY[0], panHorizon_XY[1]); //14x5
45
  }
46
  else{
47
    osd.clear();
48
    waitingMAVBeats = 1;
49
    // Display our logo and wait...
50
    panWaitMAVBeats(5,10); //Waiting for MAVBeats...
51
  }
52
// OSD debug for development (Shown on top-middle panels)
53
#ifdef membug
54
     osd.setPanel(13,4);
55
     osd.openPanel();
56
     osd.printf("%i",freeMem());
57
     osd.closePanel();
58
#endif
59
}
60
 
61
/******* PANELS - DEFINITION *******/
62
 
63
 
64
/* **************************************************************** */
65
// Panel  : panAlt
66
// Needs  : X, Y locations
67
// Output : Alt symbol and altitude value in meters from MAVLink
68
// Size   : 1 x 7Hea  (rows x chars)
69
// Staus  : done
70
 
71
void panAlt(int first_col, int first_line){
72
  osd.setPanel(first_col, first_line);
73
  osd.openPanel();
74
  //osd.printf("%c%5.0f%c",0x85, (double)(osd_alt - osd_home_alt), 0x8D);
75
  osd.printf("%c%5.0f%c",0x85, (double)(osd_alt), 0x8D);
76
  osd.closePanel();
77
}
78
 
79
/* **************************************************************** */
80
// Panel  : panVel
81
// Needs  : X, Y locations
82
// Output : Velocity value from MAVlink with symbols
83
// Size   : 1 x 7  (rows x chars)
84
// Staus  : done
85
 
86
void panVel(int first_col, int first_line){
87
  osd.setPanel(first_col, first_line);
88
  osd.openPanel();
89
  osd.printf("%c%3.0f%c",0x86,(double)osd_groundspeed,0x88);
90
  osd.closePanel();
91
}
92
 
93
/* **************************************************************** */
94
// Panel  : panThr
95
// Needs  : X, Y locations
96
// Output : Throttle value from MAVlink with symbols
97
// Size   : 1 x 7  (rows x chars)
98
// Staus  : done
99
 
100
void panThr(int first_col, int first_line){
101
  osd.setPanel(first_col, first_line);
102
  osd.openPanel();
103
  osd.printf("%c%3.0i%c",0x87,osd_throttle,0x25);
104
  osd.closePanel();
105
}
106
 
107
/* **************************************************************** */
108
// Panel  : panHomeDis
109
// Needs  : X, Y locations
110
// Output : Home Symbol with distance to home in meters
111
// Size   : 1 x 7  (rows x chars)
112
// Staus  : done
113
 
114
void panHomeDis(int first_col, int first_line){
115
  osd.setPanel(first_col, first_line);
116
  osd.openPanel();
117
  osd.printf("%c%5.0f%c", 0x1F, (double)osd_home_distance, 0x8D);
118
  osd.closePanel();
119
}
120
 
121
/* **************************************************************** */
122
// Panel  : panCenter
123
// Needs  : X, Y locations
124
// Output : 2 row croshair symbol created by 2 x 4 chars
125
// Size   : 2 x 4  (rows x chars)
126
// Staus  : done
127
 
128
void panCenter(int first_col, int first_line){
129
  osd.setPanel(first_col, first_line);
130
  osd.openPanel();
131
  osd.printf_P(PSTR("\x05\x03\x04\x05|\x15\x13\x14\x15"));
132
  osd.closePanel();
133
}
134
 
135
/* **************************************************************** */
136
// Panel  : panHorizon
137
// Needs  : X, Y locations
138
// Output : 12 x 4 Horizon line surrounded by 2 cols (left/right rules)
139
// Size   : 14 x 4  (rows x chars)
140
// Staus  : done
141
 
142
void panHorizon(int first_col, int first_line){
143
  osd.setPanel(first_col, first_line);
144
  osd.openPanel();
145
  osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
146
  osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
147
  osd.printf_P(PSTR("\xd8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xd9|"));
148
  osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
149
  osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9"));
150
  osd.closePanel();
151
  showHorizon((first_col + 1), first_line);
152
}
153
 
154
/* **************************************************************** */
155
// Panel  : panPitch
156
// Needs  : X, Y locations
157
// Output : -+ value of current Pitch from vehicle with degree symbols and pitch symbol
158
// Size   : 1 x 6  (rows x chars)
159
// Staus  : done
160
 
161
void panPitch(int first_col, int first_line){
162
  osd.setPanel(first_col, first_line);
163
  osd.openPanel();
164
  osd.printf("%4i%c%c",osd_pitch,0xb0,0xb1);
165
  osd.closePanel();
166
}
167
 
168
/* **************************************************************** */
169
// Panel  : panRoll
170
// Needs  : X, Y locations
171
// Output : -+ value of current Roll from vehicle with degree symbols and roll symbol
172
// Size   : 1 x 6  (rows x chars)
173
// Staus  : done
174
 
175
void panRoll(int first_col, int first_line){
176
  osd.setPanel(first_col, first_line);
177
  osd.openPanel();
178
  osd.printf("%4i%c%c",osd_roll,0xb0,0xb2);
179
  osd.closePanel();
180
}
181
 
182
/* **************************************************************** */
183
// Panel  : panBattery A (Voltage 1)
184
// Needs  : X, Y locations
185
// Output : Voltage value as in XX.X and symbol of over all battery status
186
// Size   : 1 x 8  (rows x chars)
187
// Staus  : done
188
 
189
void panBatt_A(int first_col, int first_line){
190
  osd.setPanel(first_col, first_line);
191
  osd.openPanel();
192
/*************** This commented code is for the next ArduPlane Version
193
#ifdef MAVLINK10
194
  if(osd_battery_remaining_A > 100){
195
    osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
196
  }
197
#else
198
  if(osd_battery_remaining_A > 1000){
199
    osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
200
  }
201
#endif //MAVLINK10
202
  else osd.printf("%c%5.2f%c%c", 0xE2, (double)osd_vbat_A, 0x8E, osd_battery_pic_A);
203
*/
204
  osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
205
  osd.closePanel();
206
}
207
 
208
//------------------ Panel: Startup ArduCam OSD LOGO -------------------------------
209
 
210
void panLogo(int first_col, int first_line){
211
  osd.setPanel(first_col, first_line);
212
  osd.openPanel();
213
  osd.printf_P(PSTR("\x20\x20\x20\x20\xba\xbb\xbc\xbd\xbe|\x20\x20\x20\x20\xca\xcb\xcc\xcd\xce|ArduCam OSD"));
214
  osd.closePanel();
215
}
216
 
217
//------------------ Panel: Waiting for MAVLink HeartBeats -------------------------------
218
 
219
void panWaitMAVBeats(int first_col, int first_line){
220
  panLogo(10,5);
221
  osd.setPanel(first_col, first_line);
222
  osd.openPanel();
223
  osd.printf_P(PSTR("Waiting for|MAVLink heartbeats..."));
224
  osd.closePanel();
225
}
226
 
227
/* **************************************************************** */
228
// Panel  : panGPL
229
// Needs  : X, Y locations
230
// Output : 1 static symbol with changing FIX symbol
231
// Size   : 1 x 2  (rows x chars)
232
// Staus  : done
233
 
234
void panGPL(int first_col, int first_line){
235
  osd.setPanel(first_col, first_line);
236
  osd.openPanel();
237
  switch(osd_fix_type) {
238
    case 0:
239
      osd.printf_P(PSTR("\x10\x20"));
240
      break;
241
    case 1:
242
      osd.printf_P(PSTR("\x10\x20"));
243
      break;
244
    case 2:
245
      osd.printf_P(PSTR("\x11\x20"));//If not APM, x01 would show 2D fix
246
      break;
247
    case 3:
248
      osd.printf_P(PSTR("\x11\x20"));//If not APM, x02 would show 3D fix
249
      break;
250
  }
251
 
252
    /*  if(osd_fix_type <= 1) {
253
    osd.printf_P(PSTR("\x10"));
254
  } else {
255
    osd.printf_P(PSTR("\x11"));
256
  }  */
257
  osd.closePanel();
258
}
259
 
260
/* **************************************************************** */
261
// Panel  : panGPSats
262
// Needs  : X, Y locations
263
// Output : 1 symbol and number of locked satellites
264
// Size   : 1 x 5  (rows x chars)
265
// Staus  : done
266
 
267
void panGPSats(int first_col, int first_line){
268
  osd.setPanel(first_col, first_line);
269
  osd.openPanel();
270
  osd.printf("%c%2i", 0x0f,osd_satellites_visible);
271
  osd.closePanel();
272
}
273
 
274
/* **************************************************************** */
275
// Panel  : panGPS
276
// Needs  : X, Y locations
277
// Output : two row numeric value of current GPS location with LAT/LON symbols as on first char
278
// Size   : 2 x 12  (rows x chars)
279
// Staus  : done
280
 
281
void panGPS(int first_col, int first_line){
282
  osd.setPanel(first_col, first_line);
283
  osd.openPanel();
284
  osd.printf("%c%11.6f|%c%11.6f", 0x83, (double)osd_lat, 0x84, (double)osd_lon);
285
  osd.closePanel();
286
}
287
 
288
/* **************************************************************** */
289
// Panel  : panHeading
290
// Needs  : X, Y locations
291
// Output : Symbols with numeric compass heading value
292
// Size   : 1 x 5  (rows x chars)
293
// Staus  : not ready
294
 
295
void panHeading(int first_col, int first_line){
296
  osd.setPanel(first_col, first_line);
297
  osd.openPanel();
298
  osd.printf("%4.0f%c", (double)osd_heading, 0xb0);
299
  osd.closePanel();
300
}
301
 
302
/* **************************************************************** */
303
// Panel  : panRose
304
// Needs  : X, Y locations
305
// Output : a dynamic compass rose that changes along the heading information
306
// Size   : 2 x 13  (rows x chars)
307
// Staus  : done
308
 
309
void panRose(int first_col, int first_line){
310
  osd.setPanel(first_col, first_line);
311
  osd.openPanel();
312
  //osd_heading  = osd_yaw;
313
  //if(osd_yaw < 0) osd_heading = 360 + osd_yaw;
314
  osd.printf("%s|%c%s%c", "\x20\xc0\xc0\xc0\xc0\xc0\xc7\xc0\xc0\xc0\xc0\xc0\x20", 0xd0, buf_show, 0xd1);
315
  osd.closePanel();
316
}
317
 
318
 
319
/* **************************************************************** */
320
// Panel  : panBoot
321
// Needs  : X, Y locations
322
// Output : Booting up text and empty bar after that
323
// Size   : 1 x 21  (rows x chars)
324
// Staus  : done
325
 
326
void panBoot(int first_col, int first_line){
327
  osd.setPanel(first_col, first_line);
328
  osd.openPanel();
329
  osd.printf_P(PSTR("Booting up:\xed\xf2\xf2\xf2\xf2\xf2\xf2\xf2\xf3"));
330
  osd.closePanel();
331
 
332
}
333
 
334
/* **************************************************************** */
335
// Panel  : panMavBeat
336
// Needs  : X, Y locations
337
// Output : 2 symbols, one static and one that blinks on every 50th received
338
//          mavlink packet.
339
// Size   : 1 x 2  (rows x chars)
340
// Staus  : done
341
 
342
void panMavBeat(int first_col, int first_line){
343
  osd.setPanel(first_col, first_line);
344
  osd.openPanel();
345
  if(mavbeat == 1){
346
    osd.printf_P(PSTR("\xEA\xEC"));
347
    mavbeat = 0;
348
  }
349
  else{
350
    osd.printf_P(PSTR("\xEA\xEB"));
351
  }
352
  osd.closePanel();
353
}
354
 
355
 
356
/* **************************************************************** */
357
// Panel  : panWPDir
358
// Needs  : X, Y locations
359
// Output : 2 symbols that are combined as one arrow, shows direction to next waypoint
360
// Size   : 1 x 2  (rows x chars)
361
// Staus  : not ready
362
 
363
void panWPDir(int first_col, int first_line){
364
  osd.setPanel(first_col, first_line);
365
  osd.openPanel();
366
  showArrow();
367
  osd.closePanel();
368
}
369
 
370
/* **************************************************************** */
371
// Panel  : panHomeDir
372
// Needs  : X, Y locations
373
// Output : 2 symbols that are combined as one arrow, shows direction to home
374
// Size   : 1 x 2  (rows x chars)
375
// Status : not tested
376
 
377
void panHomeDir(int first_col, int first_line){
378
  osd.setPanel(first_col, first_line);
379
  osd.openPanel();
380
  showArrow();
381
  osd.closePanel();
382
}
383
 
384
/* **************************************************************** */
385
// Panel  : panFlightMode
386
// Needs  : X, Y locations
387
// Output : 2 symbols, one static name symbol and another that changes by flight modes
388
// Size   : 1 x 2  (rows x chars)
389
// Status : done
390
 
391
void panFlightMode(int first_col, int first_line){
392
  osd.setPanel(first_col, first_line);
393
  osd.openPanel();
394
#ifndef MAVLINK10
395
  if(apm_mav_type == 2){//ArduCopter MultiRotor or ArduCopter Heli
396
    if(osd_mode == 100) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
397
    if(osd_mode == 101) osd.printf_P(PSTR("\xE0""acro"));//Acrobatic
398
    if(osd_mode == 102) osd.printf_P(PSTR("\xE0""alth"));//Alt Hold
399
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""auto"));//Auto
400
    if(osd_mode == MAV_MODE_GUIDED && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""guid"));//Guided
401
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_HOLD) osd.printf_P(PSTR("\xE0""loit"));//Loiter
402
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
403
    if(osd_mode == 107) osd.printf_P(PSTR("\xE0""circ")); // Circle
404
    if(osd_mode == 108) osd.printf_P(PSTR("\xE0""posi")); // Position
405
    if(osd_mode == 109) osd.printf_P(PSTR("\xE0""land")); // Land
406
    if(osd_mode == 110) osd.printf_P(PSTR("\xE0""oflo")); // OF_Loiter
407
  }
408
  else if(apm_mav_type == 1){//ArduPlane
409
    if(osd_mode == MAV_MODE_TEST1 && osd_nav_mode == MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
410
    if(osd_mode == MAV_MODE_MANUAL && osd_nav_mode == MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0""manu"));//Manual
411
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_LOITER) osd.printf_P(PSTR("\xE0""loit"));//Loiter
412
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
413
    if(osd_mode == MAV_MODE_TEST2 && osd_nav_mode == 1) osd.printf_P(PSTR("\xE0""fbwa"));//FLY_BY_WIRE_A
414
    if(osd_mode == MAV_MODE_TEST2 && osd_nav_mode == 2) osd.printf_P(PSTR("\xE0""fbwb"));//FLY_BY_WIRE_B
415
    if(osd_mode == MAV_MODE_GUIDED) osd.printf_P(PSTR("\xE0""guid"));//GUIDED
416
    if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""auto"));//AUTO
417
    if(osd_mode == MAV_MODE_TEST3) osd.printf_P(PSTR("\xE0""circ"));//CIRCLE
418
  }
419
#else
420
  if(apm_mav_type == 2){//ArduCopter MultiRotor or ArduCopter Heli
421
    if(osd_mode == 0) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
422
    if(osd_mode == 1) osd.printf_P(PSTR("\xE0""acro"));//Acrobatic
423
    if(osd_mode == 2) osd.printf_P(PSTR("\xE0""alth"));//Alt Hold
424
    if(osd_mode == 3) osd.printf_P(PSTR("\xE0""auto"));//Auto
425
    if(osd_mode == 4) osd.printf_P(PSTR("\xE0""guid"));//Guided
426
    if(osd_mode == 5) osd.printf_P(PSTR("\xE0""loit"));//Loiter
427
    if(osd_mode == 6) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
428
    if(osd_mode == 7) osd.printf_P(PSTR("\xE0""circ")); // Circle
429
    if(osd_mode == 8) osd.printf_P(PSTR("\xE0""posi")); // Position
430
    if(osd_mode == 9) osd.printf_P(PSTR("\xE0""land")); // Land
431
    if(osd_mode == 10) osd.printf_P(PSTR("\xE0""oflo")); // OF_Loiter
432
  }
433
  else if(apm_mav_type == 1){//ArduPlane
434
    if(osd_mode == 2 ) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
435
    if(osd_mode == 0) osd.printf_P(PSTR("\xE0""manu"));//Manual
436
    if(osd_mode == 12) osd.printf_P(PSTR("\xE0""loit"));//Loiter
437
    if(osd_mode == 11 ) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
438
    if(osd_mode == 5 ) osd.printf_P(PSTR("\xE0""fbwa"));//FLY_BY_WIRE_A
439
    if(osd_mode == 6 ) osd.printf_P(PSTR("\xE0""fbwb"));//FLY_BY_WIRE_B
440
    if(osd_mode == 15) osd.printf_P(PSTR("\xE0""guid"));//GUIDED
441
    if(osd_mode == 10 ) osd.printf_P(PSTR("\xE0""auto"));//AUTO
442
    if(osd_mode == 1) osd.printf_P(PSTR("\xE0""circ"));//CIRCLE
443
  }
444
#endif
445
  osd.closePanel();
446
}
447
 
448
 
449
// ---------------- EXTRA FUNCTIONS ----------------------
450
// Show those fancy 2 char arrows
451
void showArrow() {
452
   switch(osd_home_direction) {
453
    case 0:
454
      osd.printf_P(PSTR("\x90\x91"));
455
      break;
456
    case 1:
457
      osd.printf_P(PSTR("\x90\x91"));
458
      break;
459
    case 2:
460
      osd.printf_P(PSTR("\x92\x93"));
461
      break;
462
    case 3:
463
      osd.printf_P(PSTR("\x94\x95"));
464
      break;
465
    case 4:
466
      osd.printf_P(PSTR("\x96\x97"));
467
      break;
468
    case 5:
469
      osd.printf_P(PSTR("\x98\x99"));
470
      break;
471
    case 6:
472
      osd.printf_P(PSTR("\x9A\x9B"));
473
      break;
474
    case 7:
475
      osd.printf_P(PSTR("\x9C\x9D"));
476
      break;
477
    case 8:
478
      osd.printf_P(PSTR("\x9E\x9F"));
479
      break;
480
    case 9:
481
      osd.printf_P(PSTR("\xA0\xA1"));
482
      break;
483
    case 10:
484
      osd.printf_P(PSTR("\xA2\xA3"));
485
      break;
486
    case 11:
487
      osd.printf_P(PSTR("\xA4\xA5"));
488
      break;
489
    case 12:
490
      osd.printf_P(PSTR("\xA6\xA7"));
491
      break;
492
    case 13:
493
      osd.printf_P(PSTR("\xA8\xA9"));
494
      break;
495
    case 14:
496
      osd.printf_P(PSTR("\xAA\xAB"));
497
      break;
498
    case 15:
499
      osd.printf_P(PSTR("\xAC\xAD"));
500
      break;
501
    case 16:
502
      osd.printf_P(PSTR("\xAE\xAF"));
503
      break;
504
  }
505
}
506
 
507
// Calculate and shows Artificial Horizon
508
void showHorizon(int start_col, int start_row) {
509
 
510
  int x, nose, row, minval, hit, subval = 0;
511
  int cols = 12;
512
  int rows = 5;
513
  int col_hit[cols];
514
  float  pitch, roll;
515
 
516
  (abs(osd_pitch) == 90)?pitch = 89.99 * (90/osd_pitch) * -0.017453293:pitch = osd_pitch * -0.017453293;
517
  (abs(osd_roll) == 90)?roll = 89.99 * (90/osd_roll) * 0.017453293:roll = osd_roll * 0.017453293;
518
 
519
  nose = round(tan(pitch) * (rows*9));
520
  for(int col=1;col <= cols;col++){
521
    x = (col * 12) - (cols * 6) - 6;//center X point at middle of each col
522
    col_hit[col-1] = (tan(roll) * x) + nose + (rows*9) - 1;//calculating hit point on Y plus offset to eliminate negative values
523
    //col_hit[(col-1)] = nose + (rows * 9);
524
  }
525
 
526
  for(int col=0;col < cols; col++){
527
    hit = col_hit[col];
528
    if(hit > 0 && hit < (rows * 18)){
529
      row = rows - ((hit-1)/18);
530
      minval = rows*18 - row*18 + 1;
531
      subval = hit - minval;
532
      subval = round((subval*9)/18);
533
      if(subval == 0) subval = 1;
534
      printHit(start_col + col, start_row + row - 1, subval);
535
    }
536
  }
537
}
538
 
539
void printHit(byte col, byte row, byte subval){
540
  osd.openSingle(col, row);
541
    switch (subval){
542
      case 1:
543
        osd.printf_P(PSTR("\x06"));
544
        break;
545
      case 2:
546
        osd.printf_P(PSTR("\x07"));
547
        break;
548
      case 3:
549
        osd.printf_P(PSTR("\x08"));
550
        break;
551
      case 4:
552
        osd.printf_P(PSTR("\x09"));
553
        break;
554
      case 5:
555
        osd.printf_P(PSTR("\x0a"));
556
        break;
557
      case 6:
558
        osd.printf_P(PSTR("\x0b"));
559
        break;
560
      case 7:
561
        osd.printf_P(PSTR("\x0c"));
562
        break;
563
      case 8:
564
        osd.printf_P(PSTR("\x0d"));
565
        break;
566
      case 9:
567
        osd.printf_P(PSTR("\x0e"));
568
        break;
569
    }
570
}