Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1702 | - | 1 | using System; |
| 2 | using System.Collections.Generic; |
||
| 3 | using System.Linq; |
||
| 4 | using System.Text; |
||
| 5 | |||
| 6 | namespace OSD |
||
| 7 | { |
||
| 8 | // this is mainly copied from osd_panels.pde |
||
| 9 | |||
| 10 | |||
| 11 | using uint16_t = System.UInt16; |
||
| 12 | using uint8_t = System.Byte; |
||
| 13 | using int8_t = System.SByte; |
||
| 14 | using boolean = System.Byte; |
||
| 15 | |||
| 16 | class Panels |
||
| 17 | { |
||
| 18 | OSD osd; |
||
| 19 | |||
| 20 | public Panels(OSD os) |
||
| 21 | { |
||
| 22 | osd = os; |
||
| 23 | } |
||
| 24 | |||
| 25 | string PSTR(string input) |
||
| 26 | { |
||
| 27 | return input; |
||
| 28 | } |
||
| 29 | |||
| 30 | double abs(double input) |
||
| 31 | { |
||
| 32 | return Math.Abs(input); |
||
| 33 | } |
||
| 34 | |||
| 35 | int round(double input) |
||
| 36 | { |
||
| 37 | return (int)Math.Round(input, 0); |
||
| 38 | } |
||
| 39 | |||
| 40 | double tan(double input) |
||
| 41 | { |
||
| 42 | return Math.Tan(input); |
||
| 43 | } |
||
| 44 | |||
| 45 | /*Panels variables*/ |
||
| 46 | //Will come from APM telem port |
||
| 47 | |||
| 48 | |||
| 49 | static float osd_vbat = 11.61f; // voltage in milivolt |
||
| 50 | static uint16_t osd_battery_remaining = 50; // 0 to 100 <=> 0 to 1000 |
||
| 51 | static uint8_t osd_battery_pic = 0xb4; // picture to show battery remaining |
||
| 52 | |||
| 53 | static uint16_t osd_mode = 100; // Navigation mode from RC AC2 = CH5, APM = CH8 |
||
| 54 | static uint8_t osd_nav_mode = 4; // Navigation mode from RC AC2 = CH5, APM = CH8 |
||
| 55 | |||
| 56 | static float osd_lat = -35.020938f; // latidude |
||
| 57 | static float osd_lon = 117.883419f; // longitude |
||
| 58 | static uint8_t osd_satellites_visible = 7; // number of satelites |
||
| 59 | static uint8_t osd_fix_type = 3; // GPS lock 0-1=no fix, 2=2D, 3=3D |
||
| 60 | |||
| 61 | //static uint8_t osd_got_home = 0; // tels if got home position or not |
||
| 62 | //static float osd_home_lat = 0; // home latidude |
||
| 63 | //static float osd_home_lon = 0; // home longitude |
||
| 64 | //static float osd_home_alt = 0; |
||
| 65 | static long osd_home_distance = 0; // distance from home |
||
| 66 | static uint8_t osd_home_direction = 0; // Arrow direction pointing to home (1-16 to CW loop) |
||
| 67 | |||
| 68 | static int8_t osd_pitch = 0; // pitch form DCM |
||
| 69 | static int8_t osd_roll = 0; // roll form DCM |
||
| 70 | //static uint8_t osd_yaw = 0; // relative heading form DCM |
||
| 71 | static float osd_heading = 0; // ground course heading from GPS |
||
| 72 | static float osd_alt = 200; // altitude |
||
| 73 | static float osd_groundspeed = 12; // ground speed |
||
| 74 | static uint16_t osd_throttle = 52; // throtle |
||
| 75 | |||
| 76 | //MAVLink session control |
||
| 77 | static boolean mavbeat = 1; |
||
| 78 | //static float lastMAVBeat = 0; |
||
| 79 | //static boolean waitingMAVBeats = 1; |
||
| 80 | static uint8_t apm_mav_type = 2; |
||
| 81 | //static uint8_t apm_mav_system = 7; |
||
| 82 | //static uint8_t apm_mav_component = 0; |
||
| 83 | //static boolean enable_mav_request = 0; |
||
| 84 | |||
| 85 | /******* PANELS - DEFINITION *******/ |
||
| 86 | |||
| 87 | |||
| 88 | /* **************************************************************** */ |
||
| 89 | // Panel : panAlt |
||
| 90 | // Needs : X, Y locations |
||
| 91 | // Output : Alt symbol and altitude value in meters from MAVLink |
||
| 92 | // Size : 1 x 7Hea (rows x chars) |
||
| 93 | // Staus : done |
||
| 94 | |||
| 95 | public int panAlt(int first_col, int first_line) |
||
| 96 | { |
||
| 97 | osd.setPanel(first_col, first_line); |
||
| 98 | osd.openPanel(); |
||
| 99 | //osd.printf("%c%5.0f%c",0x85, (double)(osd_alt - osd_home_alt), 0x8D); |
||
| 100 | osd.printf("%c%5.0f%c", 0x85, (double)(osd_alt), 0x8D); |
||
| 101 | osd.closePanel(); |
||
| 102 | return 0; |
||
| 103 | } |
||
| 104 | |||
| 105 | /* **************************************************************** */ |
||
| 106 | // Panel : panVel |
||
| 107 | // Needs : X, Y locations |
||
| 108 | // Output : Velocity value from MAVlink with symbols |
||
| 109 | // Size : 1 x 7 (rows x chars) |
||
| 110 | // Staus : done |
||
| 111 | |||
| 112 | public int panVel(int first_col, int first_line) |
||
| 113 | { |
||
| 114 | osd.setPanel(first_col, first_line); |
||
| 115 | osd.openPanel(); |
||
| 116 | osd.printf("%c%3.0f%c", 0x86, (double)osd_groundspeed, 0x88); |
||
| 117 | osd.closePanel(); |
||
| 118 | return 0; |
||
| 119 | } |
||
| 120 | |||
| 121 | /* **************************************************************** */ |
||
| 122 | // Panel : panThr |
||
| 123 | // Needs : X, Y locations |
||
| 124 | // Output : Throttle value from MAVlink with symbols |
||
| 125 | // Size : 1 x 7 (rows x chars) |
||
| 126 | // Staus : done |
||
| 127 | |||
| 128 | public int panThr(int first_col, int first_line) |
||
| 129 | { |
||
| 130 | osd.setPanel(first_col, first_line); |
||
| 131 | osd.openPanel(); |
||
| 132 | osd.printf("%c%3.0i%c", 0x87, osd_throttle, 0x25); |
||
| 133 | osd.closePanel(); |
||
| 134 | return 0; |
||
| 135 | } |
||
| 136 | |||
| 137 | /* **************************************************************** */ |
||
| 138 | // Panel : panHomeDis |
||
| 139 | // Needs : X, Y locations |
||
| 140 | // Output : Home Symbol with distance to home in meters |
||
| 141 | // Size : 1 x 7 (rows x chars) |
||
| 142 | // Staus : done |
||
| 143 | |||
| 144 | public int panHomeDis(int first_col, int first_line) |
||
| 145 | { |
||
| 146 | osd.setPanel(first_col, first_line); |
||
| 147 | osd.openPanel(); |
||
| 148 | osd.printf("%c%5.0f%c", 0x1F, (double)osd_home_distance, 0x8D); |
||
| 149 | osd.closePanel(); |
||
| 150 | return 0; |
||
| 151 | } |
||
| 152 | |||
| 153 | /* **************************************************************** */ |
||
| 154 | // Panel : panCenter |
||
| 155 | // Needs : X, Y locations |
||
| 156 | // Output : 2 row croshair symbol created by 2 x 4 chars |
||
| 157 | // Size : 2 x 4 (rows x chars) |
||
| 158 | // Staus : done |
||
| 159 | |||
| 160 | public int panCenter(int first_col, int first_line) |
||
| 161 | { |
||
| 162 | osd.setPanel(first_col, first_line); |
||
| 163 | osd.openPanel(); |
||
| 164 | osd.printf_P(PSTR("\x05\x03\x04\x05|\x15\x13\x14\x15")); |
||
| 165 | osd.closePanel(); |
||
| 166 | return 0; |
||
| 167 | } |
||
| 168 | |||
| 169 | /* **************************************************************** */ |
||
| 170 | // Panel : panHorizon |
||
| 171 | // Needs : X, Y locations |
||
| 172 | // Output : 12 x 4 Horizon line surrounded by 2 cols (left/right rules) |
||
| 173 | // Size : 14 x 4 (rows x chars) |
||
| 174 | // Staus : done |
||
| 175 | |||
| 176 | public int panHorizon(int first_col, int first_line) |
||
| 177 | { |
||
| 178 | showHorizon((first_col + 1), first_line); |
||
| 179 | osd.setPanel(first_col, first_line); |
||
| 180 | osd.openPanel(); |
||
| 181 | osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|")); |
||
| 182 | osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|")); |
||
| 183 | osd.printf_P(PSTR("\xd8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xd9|")); |
||
| 184 | osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|")); |
||
| 185 | osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9")); |
||
| 186 | osd.closePanel(); |
||
| 187 | return 0; |
||
| 188 | } |
||
| 189 | |||
| 190 | /* **************************************************************** */ |
||
| 191 | // Panel : panPitch |
||
| 192 | // Needs : X, Y locations |
||
| 193 | // Output : -+ value of current Pitch from vehicle with degree symbols and pitch symbol |
||
| 194 | // Size : 1 x 6 (rows x chars) |
||
| 195 | // Staus : done |
||
| 196 | |||
| 197 | public int panPitch(int first_col, int first_line) |
||
| 198 | { |
||
| 199 | osd.setPanel(first_col, first_line); |
||
| 200 | osd.openPanel(); |
||
| 201 | osd.printf("%4i%c%c", osd_pitch, 0xb0, 0xb1); |
||
| 202 | osd.closePanel(); |
||
| 203 | return 0; |
||
| 204 | } |
||
| 205 | |||
| 206 | /* **************************************************************** */ |
||
| 207 | // Panel : panRoll |
||
| 208 | // Needs : X, Y locations |
||
| 209 | // Output : -+ value of current Roll from vehicle with degree symbols and roll symbol |
||
| 210 | // Size : 1 x 6 (rows x chars) |
||
| 211 | // Staus : done |
||
| 212 | |||
| 213 | public int panRoll(int first_col, int first_line) |
||
| 214 | { |
||
| 215 | osd.setPanel(first_col, first_line); |
||
| 216 | osd.openPanel(); |
||
| 217 | osd.printf("%4i%c%c", osd_roll, 0xb0, 0xb2); |
||
| 218 | osd.closePanel(); |
||
| 219 | return 0; |
||
| 220 | } |
||
| 221 | |||
| 222 | /* **************************************************************** */ |
||
| 223 | // Panel : panBattery A (Voltage 1) |
||
| 224 | // Needs : X, Y locations |
||
| 225 | // Output : Voltage value as in XX.X and symbol of over all battery status |
||
| 226 | // Size : 1 x 8 (rows x chars) |
||
| 227 | // Staus : done |
||
| 228 | |||
| 229 | public int panBatt_A(int first_col, int first_line) |
||
| 230 | { |
||
| 231 | osd.setPanel(first_col, first_line); |
||
| 232 | osd.openPanel(); |
||
| 233 | osd.printf("%c%5.2f%c%c", 0xE2,(double)osd_vbat, 0x8E, osd_battery_pic); |
||
| 234 | osd.closePanel(); |
||
| 235 | return 0; |
||
| 236 | } |
||
| 237 | |||
| 238 | //------------------ Panel: Startup ArduCam OSD LOGO ------------------------------- |
||
| 239 | |||
| 240 | public int panLogo(int first_col, int first_line) |
||
| 241 | { |
||
| 242 | osd.setPanel(first_col, first_line); |
||
| 243 | osd.openPanel(); |
||
| 244 | osd.printf_P(PSTR("\x20\x20\x20\x20\xba\xbb\xbc\xbd\xbe|\x20\x20\x20\x20\xca\xcb\xcc\xcd\xce|ArduCam OSD")); |
||
| 245 | osd.closePanel(); |
||
| 246 | return 0; |
||
| 247 | } |
||
| 248 | |||
| 249 | //------------------ Panel: Waiting for MAVLink HeartBeats ------------------------------- |
||
| 250 | |||
| 251 | public int panWaitMAVBeats(int first_col, int first_line) |
||
| 252 | { |
||
| 253 | panLogo(10, 5); |
||
| 254 | osd.setPanel(first_col, first_line); |
||
| 255 | osd.openPanel(); |
||
| 256 | osd.printf_P(PSTR("Waiting for|MAVLink heartbeats...")); |
||
| 257 | osd.closePanel(); |
||
| 258 | return 0; |
||
| 259 | } |
||
| 260 | |||
| 261 | /* **************************************************************** */ |
||
| 262 | // Panel : panGPL |
||
| 263 | // Needs : X, Y locations |
||
| 264 | // Output : 1 static symbol with changing FIX symbol |
||
| 265 | // Size : 1 x 2 (rows x chars) |
||
| 266 | // Staus : done |
||
| 267 | |||
| 268 | public int panGPL(int first_col, int first_line) |
||
| 269 | { |
||
| 270 | osd.setPanel(first_col, first_line); |
||
| 271 | osd.openPanel(); |
||
| 272 | switch (osd_fix_type) |
||
| 273 | { |
||
| 274 | case 0: |
||
| 275 | osd.printf_P(PSTR("\x10\x20")); |
||
| 276 | break; |
||
| 277 | case 1: |
||
| 278 | osd.printf_P(PSTR("\x10\x20")); |
||
| 279 | break; |
||
| 280 | case 2: |
||
| 281 | osd.printf_P(PSTR("\x11\x20"));//If not APM, x01 would show 2D fix |
||
| 282 | break; |
||
| 283 | case 3: |
||
| 284 | osd.printf_P(PSTR("\x11\x20"));//If not APM, x02 would show 3D fix |
||
| 285 | break; |
||
| 286 | } |
||
| 287 | |||
| 288 | /* if(osd_fix_type <= 1) { |
||
| 289 | osd.printf_P(PSTR("\x10")); |
||
| 290 | } else { |
||
| 291 | osd.printf_P(PSTR("\x11")); |
||
| 292 | } */ |
||
| 293 | osd.closePanel(); |
||
| 294 | return 0; |
||
| 295 | } |
||
| 296 | |||
| 297 | /* **************************************************************** */ |
||
| 298 | // Panel : panGPSats |
||
| 299 | // Needs : X, Y locations |
||
| 300 | // Output : 1 symbol and number of locked satellites |
||
| 301 | // Size : 1 x 5 (rows x chars) |
||
| 302 | // Staus : done |
||
| 303 | |||
| 304 | public int panGPSats(int first_col, int first_line) |
||
| 305 | { |
||
| 306 | osd.setPanel(first_col, first_line); |
||
| 307 | osd.openPanel(); |
||
| 308 | osd.printf("%c%2i", 0x0f, osd_satellites_visible); |
||
| 309 | osd.closePanel(); |
||
| 310 | return 0; |
||
| 311 | } |
||
| 312 | |||
| 313 | /* **************************************************************** */ |
||
| 314 | // Panel : panGPS |
||
| 315 | // Needs : X, Y locations |
||
| 316 | // Output : two row numeric value of current GPS location with LAT/LON symbols as on first char |
||
| 317 | // Size : 2 x 12 (rows x chars) |
||
| 318 | // Staus : done |
||
| 319 | |||
| 320 | public int panGPS(int first_col, int first_line) |
||
| 321 | { |
||
| 322 | osd.setPanel(first_col, first_line); |
||
| 323 | osd.openPanel(); |
||
| 324 | osd.printf("%c%11.6f|%c%11.6f", 0x83, (double)osd_lat, 0x84, (double)osd_lon); |
||
| 325 | osd.closePanel(); |
||
| 326 | return 0; |
||
| 327 | } |
||
| 328 | |||
| 329 | /* **************************************************************** */ |
||
| 330 | // Panel : panHeading |
||
| 331 | // Needs : X, Y locations |
||
| 332 | // Output : Symbols with numeric compass heading value |
||
| 333 | // Size : 1 x 5 (rows x chars) |
||
| 334 | // Staus : not ready |
||
| 335 | |||
| 336 | public int panHeading(int first_col, int first_line) |
||
| 337 | { |
||
| 338 | osd.setPanel(first_col, first_line); |
||
| 339 | osd.openPanel(); |
||
| 340 | osd.printf("%4.0f%c", (double)osd_heading, 0xb0); |
||
| 341 | osd.closePanel(); |
||
| 342 | return 0; |
||
| 343 | } |
||
| 344 | |||
| 345 | /* **************************************************************** */ |
||
| 346 | // Panel : panRose |
||
| 347 | // Needs : X, Y locations |
||
| 348 | // Output : a dynamic compass rose that changes along the heading information |
||
| 349 | // Size : 2 x 13 (rows x chars) |
||
| 350 | // Staus : done |
||
| 351 | |||
| 352 | public int panRose(int first_col, int first_line) |
||
| 353 | { |
||
| 354 | osd.setPanel(first_col, first_line); |
||
| 355 | osd.openPanel(); |
||
| 356 | //osd_heading = osd_yaw; |
||
| 357 | //if(osd_yaw < 0) osd_heading = 360 + osd_yaw; |
||
| 358 | osd.printf("%s|%c%s%c", "\x20\xc0\xc0\xc0\xc0\xc0\xc7\xc0\xc0\xc0\xc0\xc0\x20", 0xd0, Encoding.Default.GetString(buf_show), 0xd1); |
||
| 359 | osd.closePanel(); |
||
| 360 | return 0; |
||
| 361 | } |
||
| 362 | |||
| 363 | |||
| 364 | /* **************************************************************** */ |
||
| 365 | // Panel : panBoot |
||
| 366 | // Needs : X, Y locations |
||
| 367 | // Output : Booting up text and empty bar after that |
||
| 368 | // Size : 1 x 21 (rows x chars) |
||
| 369 | // Staus : done |
||
| 370 | |||
| 371 | public int panBoot(int first_col, int first_line) |
||
| 372 | { |
||
| 373 | osd.setPanel(first_col, first_line); |
||
| 374 | osd.openPanel(); |
||
| 375 | osd.printf_P(PSTR("Booting up:\xed\xf2\xf2\xf2\xf2\xf2\xf2\xf2\xf3")); |
||
| 376 | osd.closePanel(); |
||
| 377 | return 0; |
||
| 378 | |||
| 379 | } |
||
| 380 | |||
| 381 | /* **************************************************************** */ |
||
| 382 | // Panel : panMavBeat |
||
| 383 | // Needs : X, Y locations |
||
| 384 | // Output : 2 symbols, one static and one that blinks on every 50th received |
||
| 385 | // mavlink packet. |
||
| 386 | // Size : 1 x 2 (rows x chars) |
||
| 387 | // Staus : done |
||
| 388 | |||
| 389 | public int panMavBeat(int first_col, int first_line) |
||
| 390 | { |
||
| 391 | osd.setPanel(first_col, first_line); |
||
| 392 | osd.openPanel(); |
||
| 393 | if (mavbeat == 1) |
||
| 394 | { |
||
| 395 | osd.printf_P(PSTR("\xEA\xEC")); |
||
| 396 | mavbeat = 0; |
||
| 397 | } |
||
| 398 | else |
||
| 399 | { |
||
| 400 | osd.printf_P(PSTR("\xEA\xEB")); |
||
| 401 | } |
||
| 402 | osd.closePanel(); |
||
| 403 | return 0; |
||
| 404 | } |
||
| 405 | |||
| 406 | |||
| 407 | /* **************************************************************** */ |
||
| 408 | // Panel : panWPDir |
||
| 409 | // Needs : X, Y locations |
||
| 410 | // Output : 2 symbols that are combined as one arrow, shows direction to next waypoint |
||
| 411 | // Size : 1 x 2 (rows x chars) |
||
| 412 | // Staus : not ready |
||
| 413 | |||
| 414 | public int panWPDir(int first_col, int first_line) |
||
| 415 | { |
||
| 416 | osd.setPanel(first_col, first_line); |
||
| 417 | osd.openPanel(); |
||
| 418 | showArrow(); |
||
| 419 | osd.closePanel(); |
||
| 420 | return 0; |
||
| 421 | } |
||
| 422 | |||
| 423 | /* **************************************************************** */ |
||
| 424 | // Panel : panHomeDir |
||
| 425 | // Needs : X, Y locations |
||
| 426 | // Output : 2 symbols that are combined as one arrow, shows direction to home |
||
| 427 | // Size : 1 x 2 (rows x chars) |
||
| 428 | // Status : not tested |
||
| 429 | |||
| 430 | public int panHomeDir(int first_col, int first_line) |
||
| 431 | { |
||
| 432 | osd.setPanel(first_col, first_line); |
||
| 433 | osd.openPanel(); |
||
| 434 | showArrow(); |
||
| 435 | osd.closePanel(); |
||
| 436 | return 0; |
||
| 437 | } |
||
| 438 | |||
| 439 | /* **************************************************************** */ |
||
| 440 | // Panel : panFlightMode |
||
| 441 | // Needs : X, Y locations |
||
| 442 | // Output : 2 symbols, one static name symbol and another that changes by flight modes |
||
| 443 | // Size : 1 x 2 (rows x chars) |
||
| 444 | // Status : done |
||
| 445 | |||
| 446 | public int panFlightMode(int first_col, int first_line) |
||
| 447 | { |
||
| 448 | osd.setPanel(first_col, first_line); |
||
| 449 | osd.openPanel(); |
||
| 450 | if (apm_mav_type == 2)//ArduCopter |
||
| 451 | { |
||
| 452 | |||
| 453 | if (osd_mode == 100) osd.printf_P(PSTR("\xE0stab"));//Stabilize |
||
| 454 | if (osd_mode == 101) osd.printf_P(PSTR("\xE0acro"));//Acrobatic |
||
| 455 | if (osd_mode == 102) osd.printf_P(PSTR("\xE0alth"));//Alt Hold |
||
| 456 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0auto"));//Auto |
||
| 457 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0guid"));//Guided |
||
| 458 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD) osd.printf_P(PSTR("\xE0loit"));//Loiter |
||
| 459 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0retl"));//Return to Launch |
||
| 460 | if (osd_mode == 107) osd.printf_P(PSTR("\xE0circ")); // Circle |
||
| 461 | if (osd_mode == 108) osd.printf_P(PSTR("\xE0posi")); // Position |
||
| 462 | if (osd_mode == 109) osd.printf_P(PSTR("\xE0land")); // Land |
||
| 463 | if (osd_mode == 110) osd.printf_P(PSTR("\xE0oflo")); // OF_Loiter |
||
| 464 | } |
||
| 465 | else if (apm_mav_type == 1) // arduplane |
||
| 466 | { |
||
| 467 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1 && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0\xE2"));//Stabilize |
||
| 468 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0\xE3"));//Manual |
||
| 469 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER) osd.printf_P(PSTR("\xE0\xE4"));//Loiter |
||
| 470 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0\xE5"));//Return to Launch |
||
| 471 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2 && osd_nav_mode == 1) osd.printf_P(PSTR("\xE0\xE6"));//FLY_BY_WIRE_A |
||
| 472 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2 && osd_nav_mode == 2) osd.printf_P(PSTR("\xE0\xE7"));//FLY_BY_WIRE_B |
||
| 473 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED) osd.printf_P(PSTR("\xE0\xE7"));//GUIDED |
||
| 474 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0\xE7"));//AUTO |
||
| 475 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0\xE7"));//RTL |
||
| 476 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO && osd_nav_mode == (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER) osd.printf_P(PSTR("\xE0\xE7"));//LOITER |
||
| 477 | if (osd_mode == (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3) osd.printf_P(PSTR("\xE0\xE7"));//CIRCLE |
||
| 478 | } |
||
| 479 | // if(osd_mode == 3 && osd_nav_mode == 4) osd.printf_P(PSTR("\xD0\xD2")); |
||
| 480 | osd.closePanel(); |
||
| 481 | return 0; |
||
| 482 | } |
||
| 483 | |||
| 484 | |||
| 485 | // ---------------- EXTRA FUNCTIONS ---------------------- |
||
| 486 | // Show those fancy 2 char arrows |
||
| 487 | public int showArrow() |
||
| 488 | { |
||
| 489 | switch (osd_home_direction) |
||
| 490 | { |
||
| 491 | case 0: |
||
| 492 | osd.printf_P(PSTR("\x90\x91")); |
||
| 493 | break; |
||
| 494 | case 1: |
||
| 495 | osd.printf_P(PSTR("\x90\x91")); |
||
| 496 | break; |
||
| 497 | case 2: |
||
| 498 | osd.printf_P(PSTR("\x92\x93")); |
||
| 499 | break; |
||
| 500 | case 3: |
||
| 501 | osd.printf_P(PSTR("\x94\x95")); |
||
| 502 | break; |
||
| 503 | case 4: |
||
| 504 | osd.printf_P(PSTR("\x96\x97")); |
||
| 505 | break; |
||
| 506 | case 5: |
||
| 507 | osd.printf_P(PSTR("\x98\x99")); |
||
| 508 | break; |
||
| 509 | case 6: |
||
| 510 | osd.printf_P(PSTR("\x9A\x9B")); |
||
| 511 | break; |
||
| 512 | case 7: |
||
| 513 | osd.printf_P(PSTR("\x9C\x9D")); |
||
| 514 | break; |
||
| 515 | case 8: |
||
| 516 | osd.printf_P(PSTR("\x9E\x9F")); |
||
| 517 | break; |
||
| 518 | case 9: |
||
| 519 | osd.printf_P(PSTR("\xA0\xA1")); |
||
| 520 | break; |
||
| 521 | case 10: |
||
| 522 | osd.printf_P(PSTR("\xA2\xA3")); |
||
| 523 | break; |
||
| 524 | case 11: |
||
| 525 | osd.printf_P(PSTR("\xA4\xA5")); |
||
| 526 | break; |
||
| 527 | case 12: |
||
| 528 | osd.printf_P(PSTR("\xA6\xA7")); |
||
| 529 | break; |
||
| 530 | case 13: |
||
| 531 | osd.printf_P(PSTR("\xA8\xA9")); |
||
| 532 | break; |
||
| 533 | case 14: |
||
| 534 | osd.printf_P(PSTR("\xAA\xAB")); |
||
| 535 | break; |
||
| 536 | case 15: |
||
| 537 | osd.printf_P(PSTR("\xAC\xAD")); |
||
| 538 | break; |
||
| 539 | case 16: |
||
| 540 | osd.printf_P(PSTR("\xAE\xAF")); |
||
| 541 | break; |
||
| 542 | } |
||
| 543 | return 0; |
||
| 544 | } |
||
| 545 | |||
| 546 | // Calculate and shows Artificial Horizon |
||
| 547 | public int showHorizon(int start_col, int start_row) |
||
| 548 | { |
||
| 549 | |||
| 550 | int x, nose, row, minval, hit, subval = 0; |
||
| 551 | int cols = 12; |
||
| 552 | int rows = 5; |
||
| 553 | int[] col_hit = new int[cols]; |
||
| 554 | double pitch, roll; |
||
| 555 | |||
| 556 | if (abs(osd_pitch) == 90) { pitch = 89.99 * (90 / osd_pitch) * -0.017453293; } else { pitch = osd_pitch * -0.017453293; } |
||
| 557 | if (abs(osd_roll) == 90) { roll = 89.99 * (90 / osd_roll) * 0.017453293; } else { roll = osd_roll * 0.017453293; } |
||
| 558 | |||
| 559 | nose = round(tan(pitch) * (rows * 9)); |
||
| 560 | for (int col = 1; col <= cols; col++) |
||
| 561 | { |
||
| 562 | x = (col * 12) - (cols * 6) - 6;//center X point at middle of each col |
||
| 563 | col_hit[col - 1] = (int)(tan(roll) * x) + nose + (rows * 9) - 1;//calculating hit point on Y plus offset to eliminate negative values |
||
| 564 | //col_hit[(col-1)] = nose + (rows * 9); |
||
| 565 | } |
||
| 566 | |||
| 567 | for (int col = 0; col < cols; col++) |
||
| 568 | { |
||
| 569 | hit = col_hit[col]; |
||
| 570 | if (hit > 0 && hit < (rows * 18)) |
||
| 571 | { |
||
| 572 | row = rows - ((hit - 1) / 18); |
||
| 573 | minval = rows * 18 - row * 18 + 1; |
||
| 574 | subval = hit - minval; |
||
| 575 | subval = round((subval * 9) / 18); |
||
| 576 | if (subval == 0) subval = 1; |
||
| 577 | printHit((byte)(start_col + col), (byte)(start_row + row - 1), (byte)subval); |
||
| 578 | } |
||
| 579 | } |
||
| 580 | return 0; |
||
| 581 | } |
||
| 582 | |||
| 583 | public int printHit(byte col, byte row, byte subval) |
||
| 584 | { |
||
| 585 | osd.openSingle(col, row); |
||
| 586 | switch (subval) |
||
| 587 | { |
||
| 588 | case 1: |
||
| 589 | osd.printf_P(PSTR("\x06")); |
||
| 590 | break; |
||
| 591 | case 2: |
||
| 592 | osd.printf_P(PSTR("\x07")); |
||
| 593 | break; |
||
| 594 | case 3: |
||
| 595 | osd.printf_P(PSTR("\x08")); |
||
| 596 | break; |
||
| 597 | case 4: |
||
| 598 | osd.printf_P(PSTR("\x09")); |
||
| 599 | break; |
||
| 600 | case 5: |
||
| 601 | osd.printf_P(PSTR("\x0a")); |
||
| 602 | break; |
||
| 603 | case 6: |
||
| 604 | osd.printf_P(PSTR("\x0b")); |
||
| 605 | break; |
||
| 606 | case 7: |
||
| 607 | osd.printf_P(PSTR("\x0c")); |
||
| 608 | break; |
||
| 609 | case 8: |
||
| 610 | osd.printf_P(PSTR("\x0d")); |
||
| 611 | break; |
||
| 612 | case 9: |
||
| 613 | osd.printf_P(PSTR("\x0e")); |
||
| 614 | break; |
||
| 615 | } |
||
| 616 | return 0; |
||
| 617 | } |
||
| 618 | |||
| 619 | |||
| 620 | |||
| 621 | //------------------ Heading and Compass ---------------------------------------- |
||
| 622 | |||
| 623 | byte[] buf_show = new byte[11]; |
||
| 624 | byte[] buf_Rule = {0xc2,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0, |
||
| 625 | 0xc4,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0, |
||
| 626 | 0xc3,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0, |
||
| 627 | 0xc5,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0}; |
||
| 628 | public void setHeadingPatern() |
||
| 629 | { |
||
| 630 | int start; |
||
| 631 | start = round((osd_heading * 36) / 360); |
||
| 632 | start -= 5; |
||
| 633 | if (start < 0) start += 36; |
||
| 634 | for (int x = 0; x <= 10; x++) |
||
| 635 | { |
||
| 636 | buf_show[x] = buf_Rule[start]; |
||
| 637 | if (++start > 35) start = 0; |
||
| 638 | } |
||
| 639 | // buf_show[11] = (byte)'\0'; |
||
| 640 | } |
||
| 641 | |||
| 642 | //------------------ Battery Remaining Picture ---------------------------------- |
||
| 643 | |||
| 644 | public void setBatteryPic() |
||
| 645 | { |
||
| 646 | if (osd_battery_remaining <= 270) |
||
| 647 | { |
||
| 648 | osd_battery_pic = 0xb4; |
||
| 649 | } |
||
| 650 | else if (osd_battery_remaining <= 300) |
||
| 651 | { |
||
| 652 | osd_battery_pic = 0xb5; |
||
| 653 | } |
||
| 654 | else if (osd_battery_remaining <= 400) |
||
| 655 | { |
||
| 656 | osd_battery_pic = 0xb6; |
||
| 657 | } |
||
| 658 | else if (osd_battery_remaining <= 500) |
||
| 659 | { |
||
| 660 | osd_battery_pic = 0xb7; |
||
| 661 | } |
||
| 662 | else if (osd_battery_remaining <= 800) |
||
| 663 | { |
||
| 664 | osd_battery_pic = 0xb8; |
||
| 665 | } |
||
| 666 | else osd_battery_pic = 0xb9; |
||
| 667 | } |
||
| 668 | |||
| 669 | } |
||
| 670 | } |