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