git.gag.com Git - fw/altos/blob - src/product/ao_terraui.c

   1 /*
   2  * Copyright © 2011 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include "ao.h"
  19 #include <ao_flight.h>
  20 #include <math.h>
  21
  22 static __xdata struct ao_telemetry_sensor               ao_tel_sensor;
  23 static __xdata struct ao_telemetry_location             ao_tel_location;
  24 static __xdata struct ao_telemetry_configuration        ao_tel_config;
  25 static __xdata int16_t                                  ao_tel_max_speed;
  26 static __xdata int16_t                                  ao_tel_max_height;
  27 static int8_t ao_tel_rssi;
  28
  29 static __xdata char ao_lcd_line[17];
  30 static __xdata char ao_state_name[] = "SIPBFCDMLI";
  31
  32 static void
  33 ao_terraui_line(uint8_t addr)
  34 {
  35         ao_lcd_goto(addr);
  36         ao_lcd_putstring(ao_lcd_line);
  37 }
  38
  39 #define ao_terraui_state()      (ao_state_name[ao_tel_sensor.state])
  40
  41 static char
  42 ao_terraui_igniter(int16_t sense)
  43 {
  44         if (sense < AO_IGNITER_OPEN)
  45                 return '-';
  46         if (sense > AO_IGNITER_CLOSED)
  47                 return '+';
  48         return '?';
  49 }
  50
  51 static char
  52 ao_terraui_battery(void)
  53 {
  54         if (ao_tel_sensor.v_batt > 25558)
  55                 return '+';
  56         return '-';
  57 }
  58
  59 static char
  60 ao_terraui_gps(void)
  61 {
  62         if (ao_tel_location.flags & (1 << 4)) {
  63                 if ((ao_tel_location.flags & 0xf) >= 4)
  64                         return '+';
  65         }
  66         return '-';
  67 }
  68
  69 static char
  70 ao_terraui_local_gps(void)
  71 {
  72         if (ao_gps_data.flags & (1 << 4)) {
  73                 if ((ao_gps_data.flags & 0xf) >= 4)
  74                         return '+';
  75         }
  76         return '-';
  77 }
  78
  79 static char
  80 ao_terraui_logging(void)
  81 {
  82         if (ao_tel_config.flight != 0)
  83                 return '+';
  84         return '-';
  85 }
  86
  87 static __code char ao_progress[4] = { '\011', '\012', '\014', '\013' };
  88
  89 static uint8_t  ao_telem_progress;
  90 static uint8_t  ao_gps_progress;
  91
  92 static void
  93 ao_terraui_startup(void)
  94 {
  95         sprintf(ao_lcd_line, "%-16.16s", ao_product);
  96         ao_terraui_line(AO_LCD_ADDR(0,0));
  97         sprintf(ao_lcd_line, "%-8.8s %5u  ", ao_version, ao_serial_number);
  98         ao_terraui_line(AO_LCD_ADDR(1,0));
  99 }
 100
 101 static void
 102 ao_terraui_info_firstline(void)
 103 {
 104         sprintf(ao_lcd_line, "S %4d %7.7s %c",
 105                 ao_tel_sensor.serial,
 106                 ao_tel_config.callsign,
 107                 ao_terraui_state());
 108         ao_terraui_line(AO_LCD_ADDR(0,0));
 109 }
 110
 111 static void
 112 ao_terraui_info(void)
 113 {
 114         ao_terraui_info_firstline();
 115         sprintf(ao_lcd_line, "F %4d RSSI%4d%c",
 116                 ao_tel_config.flight,
 117                 ao_tel_rssi,
 118                 ao_progress[ao_telem_progress]);
 119         ao_terraui_line(AO_LCD_ADDR(1,0));
 120 }
 121
 122 static void
 123 ao_terraui_pad(void)
 124 {
 125         sprintf(ao_lcd_line, "B%c A%c M%c L%c G%c %c",
 126                 ao_terraui_battery(),
 127                 ao_terraui_igniter(ao_tel_sensor.sense_d),
 128                 ao_terraui_igniter(ao_tel_sensor.sense_m),
 129                 ao_terraui_logging(),
 130                 ao_terraui_gps(),
 131                 ao_terraui_state());
 132         ao_terraui_line(AO_LCD_ADDR(0,0));
 133         sprintf(ao_lcd_line, "SAT %2d RSSI%4d%c",
 134                 ao_tel_location.flags & 0xf,
 135                 ao_tel_rssi,
 136                 ao_progress[ao_telem_progress]);
 137         ao_terraui_line(AO_LCD_ADDR(1,0));
 138 }
 139
 140 static void
 141 ao_terraui_ascent(void)
 142 {
 143         sprintf(ao_lcd_line, "S %5d S\011%5d%c",
 144                 ao_tel_sensor.speed >> 4,
 145                 ao_tel_max_speed >> 4,
 146                 ao_terraui_state());
 147         ao_terraui_line(AO_LCD_ADDR(0,0));
 148         sprintf(ao_lcd_line, "H %5d H\011%5d%c",
 149                 ao_tel_sensor.height >> 4,
 150                 ao_tel_max_height >> 4,
 151                 ao_progress[ao_telem_progress]);
 152         ao_terraui_line(AO_LCD_ADDR(1,0));
 153 }
 154
 155 static int16_t mag(int32_t d)
 156 {
 157         if (d < 0)
 158                 d = -d;
 159         if (d > 0x7fff)
 160                 d = 0x7fff;
 161         return d;
 162 }
 163
 164 static int32_t
 165 dist(int32_t d)
 166 {
 167         __pdata uint32_t m;
 168         uint8_t neg = 0;
 169
 170         if (d < 0) {
 171                 d = -d;
 172                 neg = 1;
 173         }
 174
 175         m = 10000000;
 176         while (d >= (2147483647 / 111198)) {
 177                 d /= 10;
 178                 m /= 10;
 179         }
 180         d = (d * 111198) / m;
 181         if (neg)
 182                 d = -d;
 183         return d;
 184 }
 185
 186 static __code uint8_t cos_table[] = {
 187    0, /*  0 */
 188    0, /*  1 */
 189    0, /*  2 */
 190  255, /*  3 */
 191  254, /*  4 */
 192  253, /*  5 */
 193  252, /*  6 */
 194  251, /*  7 */
 195  249, /*  8 */
 196  247, /*  9 */
 197  245, /* 10 */
 198  243, /* 11 */
 199  240, /* 12 */
 200  238, /* 13 */
 201  235, /* 14 */
 202  232, /* 15 */
 203  228, /* 16 */
 204  225, /* 17 */
 205  221, /* 18 */
 206  217, /* 19 */
 207  213, /* 20 */
 208  209, /* 21 */
 209  205, /* 22 */
 210  200, /* 23 */
 211  195, /* 24 */
 212  190, /* 25 */
 213  185, /* 26 */
 214  180, /* 27 */
 215  175, /* 28 */
 216  169, /* 29 */
 217  163, /* 30 */
 218  158, /* 31 */
 219  152, /* 32 */
 220  145, /* 33 */
 221  139, /* 34 */
 222  133, /* 35 */
 223  126, /* 36 */
 224  120, /* 37 */
 225  113, /* 38 */
 226  106, /* 39 */
 227  100, /* 40 */
 228   93, /* 41 */
 229   86, /* 42 */
 230   79, /* 43 */
 231   71, /* 44 */
 232   64, /* 45 */
 233   57, /* 46 */
 234   49, /* 47 */
 235   42, /* 48 */
 236   35, /* 49 */
 237   27, /* 50 */
 238   20, /* 51 */
 239   12, /* 52 */
 240    5, /* 53 */
 241    1, /* 54 */
 242 };
 243
 244 static __code uint8_t tan_table[] = {
 245     0, /*  0 */
 246     4, /*  1 */
 247     9, /*  2 */
 248    13, /*  3 */
 249    18, /*  4 */
 250    22, /*  5 */
 251    27, /*  6 */
 252    31, /*  7 */
 253    36, /*  8 */
 254    41, /*  9 */
 255    45, /* 10 */
 256    50, /* 11 */
 257    54, /* 12 */
 258    59, /* 13 */
 259    64, /* 14 */
 260    69, /* 15 */
 261    73, /* 16 */
 262    78, /* 17 */
 263    83, /* 18 */
 264    88, /* 19 */
 265    93, /* 20 */
 266    98, /* 21 */
 267   103, /* 22 */
 268   109, /* 23 */
 269   114, /* 24 */
 270   119, /* 25 */
 271   125, /* 26 */
 272   130, /* 27 */
 273   136, /* 28 */
 274   142, /* 29 */
 275   148, /* 30 */
 276   154, /* 31 */
 277   160, /* 32 */
 278   166, /* 33 */
 279   173, /* 34 */
 280   179, /* 35 */
 281   186, /* 36 */
 282   193, /* 37 */
 283   200, /* 38 */
 284   207, /* 39 */
 285   215, /* 40 */
 286   223, /* 41 */
 287   231, /* 42 */
 288   239, /* 43 */
 289   247, /* 44 */
 290 };
 291
 292 int16_t ao_atan2(int32_t dy, int32_t dx) __reentrant
 293 {
 294         int8_t  m = 1;
 295         int16_t a = 0;
 296         uint8_t r;
 297         int8_t  t;
 298
 299         if (dx == 0) {
 300                 if (dy > 0)
 301                         return 90;
 302                 if (dy < 0)
 303                         return -90;
 304                 return 0;
 305         }
 306
 307         if (dx < 0) {
 308                 a = 180;
 309                 m = -m;
 310                 dx = -dx;
 311         }
 312
 313         if (dy < 0) {
 314                 m = -m;
 315                 a = -a;
 316                 dy = -dy;
 317         }
 318
 319         if (dy > dx) {
 320                 int32_t t;
 321
 322                 t = dy; dy = dx; dx = t;
 323                 a = a + m * 90;
 324                 m = -m;
 325         }
 326
 327         dy = dy << 8;
 328         dy = (dy + (dx >> 1)) / dx;
 329         r = dy;
 330         for (t = 0; t < 44; t++)
 331                 if (tan_table[t] >= r)
 332                         break;
 333         return t * m + a;
 334 }
 335
 336 static __pdata int32_t  lon_dist, lat_dist;
 337 static __pdata uint32_t ground_dist, range;
 338 static __pdata uint8_t dist_in_km;
 339 static __pdata int16_t bearing, elevation;
 340
 341 static void
 342 ao_terraui_lat_dist(void)
 343 {
 344         lat_dist = dist (ao_tel_location.latitude - ao_gps_data.latitude);
 345 }
 346
 347 static void
 348 ao_terraui_lon_dist(void) __reentrant
 349 {
 350         uint8_t c = cos_table[ao_gps_data.latitude >> 24];
 351         lon_dist = ao_tel_location.longitude;
 352
 353         /* check if it's shorter to go the other way around */
 354         if ((int16_t) (lon_dist >> 24) < (int16_t) (ao_gps_data.longitude >> 24) - (int16_t) (1800000000 >> 24))
 355                 lon_dist += 3600000000ul;
 356         lon_dist = dist(lon_dist - ao_gps_data.longitude);
 357         if (c) {
 358                 if (lon_dist & 0x7f800000)
 359                         lon_dist = (lon_dist >> 8) * c;
 360                 else
 361                         lon_dist = (lon_dist * (int16_t) c) >> 8;
 362         }
 363 }
 364
 365 static int32_t sqr(int32_t x) { return x * x; }
 366
 367 static void
 368 ao_terraui_compute(void)
 369 {
 370         uint16_t        h = ao_tel_sensor.height;
 371
 372         ao_terraui_lat_dist();
 373         ao_terraui_lon_dist();
 374         if (lat_dist > 32767 || lon_dist > 32767) {
 375                 dist_in_km = 1;
 376                 ground_dist = sqr(lat_dist/1000) + sqr(lon_dist/1000);
 377                 h = h/1000;
 378         } else {
 379                 dist_in_km = 0;
 380                 ground_dist = sqr(lat_dist) + sqr(lon_dist);
 381         }
 382         ground_dist = ao_sqrt (ground_dist);
 383         range = ao_sqrt(ground_dist * ground_dist + ao_tel_sensor.height * ao_tel_sensor.height);
 384         bearing = ao_atan2(lon_dist, lat_dist);
 385         if (bearing < 0)
 386                 bearing += 360;
 387         elevation = ao_atan2(ao_tel_sensor.height, ground_dist);
 388 }
 389
 390 static void
 391 ao_terraui_descent(void)
 392 {
 393         ao_terraui_compute();
 394         sprintf(ao_lcd_line, "\007 %4d  \005 %3d  %c",
 395                 bearing, elevation,
 396                 ao_terraui_state());
 397         ao_terraui_line(AO_LCD_ADDR(0,0));
 398         sprintf(ao_lcd_line, "H:%5d S:%5d%c",
 399                 ao_tel_sensor.height, ao_tel_sensor.speed >> 4,
 400                 ao_progress[ao_telem_progress]);
 401         ao_terraui_line(AO_LCD_ADDR(1,0));
 402 }
 403
 404 static void
 405 ao_terraui_recover(void)
 406 {
 407         ao_terraui_compute();
 408         sprintf(ao_lcd_line, "\007 %4d  \005 %3d  %c",
 409                 bearing, elevation,
 410                 ao_terraui_state());
 411         ao_terraui_line(AO_LCD_ADDR(0,0));
 412         sprintf(ao_lcd_line, "R%5ld%cRSSI%4d%c",
 413                 ground_dist, dist_in_km ? 'k' : ' ',
 414                 ao_tel_rssi,
 415                 ao_progress[ao_telem_progress]);
 416         ao_terraui_line(AO_LCD_ADDR(1,0));
 417 }
 418
 419 static void
 420 ao_terraui_coord(int32_t c, char plus, char minus, char extra) __reentrant
 421 {
 422         uint16_t        d;
 423         uint8_t         m;
 424         uint16_t        f;
 425
 426         if (c < 0) {
 427                 plus = minus;
 428                 c = -c;
 429         }
 430         d = c / 10000000;
 431         c = c % 10000000;
 432         c = c * 60;
 433         m = c / 10000000;
 434         c = c % 10000000;
 435         f = (c + 500) / 1000;
 436         sprintf(ao_lcd_line, "%c %3d\362 %2d.%04d\"%c",
 437                 plus, d, m, f, extra);
 438 }
 439
 440 static void
 441 ao_terraui_remote(void)
 442 {
 443         ao_terraui_coord(ao_tel_location.latitude, 'N', 'S', ao_terraui_state());
 444         ao_terraui_line(AO_LCD_ADDR(0,0));
 445         ao_terraui_coord(ao_tel_location.longitude, 'E', 'W', ao_progress[ao_telem_progress]);
 446         ao_terraui_line(AO_LCD_ADDR(1,0));
 447 }
 448
 449 static void
 450 ao_terraui_local(void) __reentrant
 451 {
 452         ao_terraui_coord(ao_gps_data.latitude, 'n', 's',
 453                          ao_terraui_local_gps());
 454         ao_terraui_line(AO_LCD_ADDR(0,0));
 455         ao_terraui_coord(ao_gps_data.longitude, 'e', 'w', ao_progress[ao_gps_progress]);
 456         ao_terraui_line(AO_LCD_ADDR(1,0));
 457 }
 458
 459 static __pdata uint8_t ao_set_freq;
 460 static __pdata uint32_t ao_set_freq_orig;
 461
 462 static void
 463 ao_terraui_freq(void) __reentrant
 464 {
 465         uint16_t        MHz;
 466         uint16_t        frac;
 467
 468         MHz = ao_config.frequency / 1000;
 469         frac = ao_config.frequency % 1000;
 470         ao_terraui_info_firstline();
 471         sprintf(ao_lcd_line, "Freq: %3d.%03d MHz", MHz, frac);
 472         ao_terraui_line(AO_LCD_ADDR(1,0));
 473         ao_lcd_goto(AO_LCD_ADDR(1,11));
 474 }
 475
 476 static void
 477 ao_terraui_freq_start(void)
 478 {
 479         ao_set_freq = 1;
 480         ao_set_freq_orig = ao_config.frequency;
 481         ao_lcd_cursor_on();
 482 }
 483
 484 static void
 485 ao_terraui_freq_button(char b) {
 486
 487         switch (b) {
 488         case 0:
 489                 return;
 490         case 1:
 491                 if (ao_config.frequency > 430000)
 492                         ao_config.frequency -= 100;
 493                 break;
 494         case 2:
 495                 ao_set_freq = 0;
 496                 ao_lcd_cursor_off();
 497                 if (ao_set_freq_orig != ao_config.frequency)
 498                         ao_config_put();
 499                 return;
 500         case 3:
 501                 if (ao_config.frequency < 438000)
 502                         ao_config.frequency += 100;
 503                 break;
 504
 505         }
 506         ao_config_set_radio();
 507         ao_radio_recv_abort();
 508 }
 509
 510 static __code void (*__code ao_terraui_page[])(void) = {
 511         ao_terraui_startup,
 512         ao_terraui_info,
 513         ao_terraui_pad,
 514         ao_terraui_ascent,
 515         ao_terraui_descent,
 516         ao_terraui_recover,
 517         ao_terraui_remote,
 518         ao_terraui_local,
 519 };
 520
 521 #define NUM_PAGE        (sizeof (ao_terraui_page)/sizeof (ao_terraui_page[0]))
 522
 523 static __pdata uint8_t ao_current_page = 0;
 524 static __pdata uint8_t ao_shown_about = 3;
 525
 526 static void
 527 ao_terraui(void)
 528 {
 529         ao_lcd_start();
 530
 531         ao_delay(AO_MS_TO_TICKS(100));
 532         ao_button_clear();
 533
 534         for (;;) {
 535                 char    b;
 536
 537                 if (ao_set_freq)
 538                         ao_terraui_freq();
 539                 else
 540                         ao_terraui_page[ao_current_page]();
 541
 542                 b = ao_button_get(AO_SEC_TO_TICKS(1));
 543
 544                 if (b > 0) {
 545                         ao_beep_for(AO_BEEP_HIGH, AO_MS_TO_TICKS(10));
 546                         ao_shown_about = 0;
 547                 }
 548
 549                 if (ao_set_freq) {
 550                         ao_terraui_freq_button(b);
 551                         continue;
 552                 }
 553                 switch (b) {
 554                 case 0:
 555                         if (ao_shown_about) {
 556                                 if (--ao_shown_about == 0)
 557                                         ao_current_page = 2;
 558                         }
 559                         break;
 560                 case 1:
 561                         ao_current_page++;
 562                         if (ao_current_page >= NUM_PAGE)
 563                                 ao_current_page = 0;
 564                         break;
 565                 case 2:
 566                         ao_terraui_freq_start();
 567                         break;
 568                 case 3:
 569                         if (ao_current_page == 0)
 570                                 ao_current_page = NUM_PAGE;
 571                         ao_current_page--;
 572                         break;
 573                 }
 574         }
 575 }
 576
 577 __xdata static struct ao_task ao_terraui_task;
 578
 579 static void
 580 ao_terramonitor(void)
 581 {
 582         uint8_t monitor;
 583
 584         monitor = ao_monitor_head;
 585         ao_monitor_set(sizeof (struct ao_telemetry_generic));
 586         for (monitor = ao_monitor_head;;
 587              monitor = ao_monitor_ring_next(monitor))
 588         {
 589                 while (monitor == ao_monitor_head)
 590                         ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
 591                 if (ao_monitoring != sizeof (union ao_telemetry_all))
 592                         continue;
 593                 if (!(ao_monitor_ring[monitor].all.status & PKT_APPEND_STATUS_1_CRC_OK))
 594                         continue;
 595                 ao_tel_rssi = AO_RSSI_FROM_RADIO(ao_monitor_ring[monitor].all.rssi);
 596                 switch (ao_monitor_ring[monitor].all.telemetry.generic.type) {
 597                 case AO_TELEMETRY_SENSOR_TELEMETRUM:
 598                 case AO_TELEMETRY_SENSOR_TELEMINI:
 599                 case AO_TELEMETRY_SENSOR_TELENANO:
 600                         ao_xmemcpy(&ao_tel_sensor, &ao_monitor_ring[monitor], sizeof (ao_tel_sensor));
 601                         if (ao_tel_sensor.state < ao_flight_boost) {
 602                                 ao_tel_max_speed = 0;
 603                                 ao_tel_max_height = 0;
 604                         } else {
 605                                 if (ao_tel_sensor.speed > ao_tel_max_speed)
 606                                         ao_tel_max_speed = ao_tel_sensor.speed;
 607                                 if (ao_tel_sensor.height > ao_tel_max_height)
 608                                         ao_tel_max_height = ao_tel_sensor.height;
 609                         }
 610                         ao_telem_progress = (ao_telem_progress + 1) & 0x3;
 611                         break;
 612                 case AO_TELEMETRY_LOCATION:
 613                         ao_xmemcpy(&ao_tel_location, &ao_monitor_ring[monitor], sizeof (ao_tel_location));
 614                         break;
 615                 case AO_TELEMETRY_CONFIGURATION:
 616                         ao_xmemcpy(&ao_tel_config, &ao_monitor_ring[monitor], sizeof (ao_tel_config));
 617                 }
 618         }
 619 }
 620
 621 __xdata static struct ao_task ao_terramonitor_task;
 622
 623 static void
 624 ao_terragps(void)
 625 {
 626         uint16_t        gps_tick = ao_gps_progress;
 627
 628         for (;;) {
 629                 while (ao_gps_tick == gps_tick)
 630                         ao_sleep(&ao_gps_new);
 631                 gps_tick = ao_gps_tick;
 632                 ao_gps_progress = (ao_gps_progress + 1) & 3;
 633         }
 634 }
 635
 636 __xdata static struct ao_task ao_terragps_task;
 637
 638 void
 639 ao_terraui_init(void)
 640 {
 641         ao_add_task(&ao_terraui_task, ao_terraui, "ui");
 642         ao_add_task(&ao_terramonitor_task, ao_terramonitor, "monitor");
 643         ao_add_task(&ao_terragps_task, ao_terragps, "gps");
 644 }