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