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