2 * Copyright © 2011 Keith Packard <keithp@keithp.com>
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.
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.
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.
20 #include <ao_flight.h>
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;
30 static char ao_lcd_line[17];
31 static char ao_state_name[] = "SIPBFCDMLI";
34 ao_terraui_line(uint8_t addr)
37 ao_lcd_putstring(ao_lcd_line);
40 #define ao_terraui_state() (ao_state_name[ao_tel_sensor.state])
43 ao_terraui_igniter(int16_t sense)
45 if (sense < AO_IGNITER_OPEN)
47 if (sense > AO_IGNITER_CLOSED)
53 ao_terraui_battery(void)
55 if (ao_tel_sensor.v_batt > 25558)
63 if (ao_tel_location.flags & (1 << 4)) {
64 if ((ao_tel_location.flags & 0xf) >= 4)
71 ao_terraui_local_gps(void)
73 if (ao_gps_data.flags & (1 << 4)) {
74 if ((ao_gps_data.flags & 0xf) >= 4)
81 ao_terraui_logging(void)
83 if (ao_tel_config.flight != 0)
88 static const char ao_progress[4] = { '\011', '\012', '\014', '\013' };
90 static uint8_t ao_telem_progress;
91 static uint8_t ao_gps_progress;
94 ao_terraui_startup(void)
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));
103 ao_terraui_info_firstline(void)
105 sprintf(ao_lcd_line, "S %4d %7.7s %c",
106 ao_tel_sensor.serial,
107 ao_tel_config.callsign,
109 ao_terraui_line(AO_LCD_ADDR(0,0));
113 ao_terraui_info(void)
115 ao_terraui_info_firstline();
116 sprintf(ao_lcd_line, "F %4d RSSI%4d%c",
117 ao_tel_config.flight,
119 ao_progress[ao_telem_progress]);
120 ao_terraui_line(AO_LCD_ADDR(1,0));
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(),
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,
137 ao_progress[ao_telem_progress]);
138 ao_terraui_line(AO_LCD_ADDR(1,0));
142 ao_terraui_ascent(void)
144 sprintf(ao_lcd_line, "S %5d S\011%5d%c",
145 ao_tel_sensor.speed >> 4,
146 ao_tel_max_speed >> 4,
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));
156 static int16_t mag(int32_t d)
177 while (d >= (2147483647 / 111198)) {
181 d = (d * 111198) / m;
187 static const uint8_t cos_table[] = {
245 static const uint8_t tan_table[] = {
293 int16_t ao_atan2(int32_t dy, int32_t dx)
323 t = dy; dy = dx; dx = t;
329 dy = (dy + (dx >> 1)) / dx;
331 for (t = 0; t < 44; t++)
332 if (tan_table[t] >= r)
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;
343 ao_terraui_lat_dist(void)
345 lat_dist = dist (ao_tel_location.latitude - ao_gps_data.latitude);
349 ao_terraui_lon_dist(void)
351 uint8_t c = cos_table[ao_gps_data.latitude >> 24];
352 lon_dist = ao_tel_location.longitude;
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);
359 if (lon_dist & 0x7f800000)
360 lon_dist = (lon_dist >> 8) * c;
362 lon_dist = (lon_dist * (int16_t) c) >> 8;
366 static int32_t sqr(int32_t x) { return x * x; }
369 ao_terraui_compute(void)
371 uint16_t h = ao_tel_sensor.height;
373 ao_terraui_lat_dist();
374 ao_terraui_lon_dist();
375 if (lat_dist > 32767 || lon_dist > 32767) {
377 ground_dist = sqr(lat_dist/1000) + sqr(lon_dist/1000);
381 ground_dist = sqr(lat_dist) + sqr(lon_dist);
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);
388 elevation = ao_atan2(ao_tel_sensor.height, ground_dist);
392 ao_terraui_descent(void)
394 ao_terraui_compute();
395 sprintf(ao_lcd_line, "\007 %4d \005 %3d %c",
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));
406 ao_terraui_recover(void)
408 ao_terraui_compute();
409 sprintf(ao_lcd_line, "\007 %4d \005 %3d %c",
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' : ' ',
416 ao_progress[ao_telem_progress]);
417 ao_terraui_line(AO_LCD_ADDR(1,0));
421 ao_terraui_coord(int32_t c, char plus, char minus, char extra)
436 f = (c + 500) / 1000;
437 sprintf(ao_lcd_line, "%c %3d\362 %2d.%04d\"%c",
438 plus, d, m, f, extra);
442 ao_terraui_remote(void)
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));
451 ao_terraui_local(void)
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));
460 static uint8_t ao_set_freq;
461 static uint32_t ao_set_freq_orig;
464 ao_terraui_freq(void)
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));
478 ao_terraui_freq_start(void)
481 ao_set_freq_orig = ao_config.frequency;
486 ao_terraui_freq_button(char b) {
492 if (ao_config.frequency > 430000)
493 ao_config.frequency -= 100;
498 if (ao_set_freq_orig != ao_config.frequency)
502 if (ao_config.frequency < 438000)
503 ao_config.frequency += 100;
507 ao_config_set_radio();
508 ao_radio_recv_abort();
511 static const void (*const ao_terraui_page[])(void) = {
522 #define NUM_PAGE (sizeof (ao_terraui_page)/sizeof (ao_terraui_page[0]))
524 static uint8_t ao_current_page = 0;
525 static uint8_t ao_shown_about = 3;
532 ao_delay(AO_MS_TO_TICKS(100));
541 ao_terraui_page[ao_current_page]();
543 b = ao_button_get(AO_SEC_TO_TICKS(1));
546 ao_beep_for(AO_BEEP_HIGH, AO_MS_TO_TICKS(10));
551 ao_terraui_freq_button(b);
556 if (ao_shown_about) {
557 if (--ao_shown_about == 0)
563 if (ao_current_page >= NUM_PAGE)
567 ao_terraui_freq_start();
570 if (ao_current_page == 0)
571 ao_current_page = NUM_PAGE;
578 static struct ao_task ao_terraui_task;
581 ao_terramonitor(void)
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))
590 while (monitor == ao_monitor_head)
591 ao_sleep(&ao_monitor_head);
592 if (ao_monitoring != sizeof (union ao_telemetry_all))
594 if (!(ao_monitor_ring[monitor].all.status & PKT_APPEND_STATUS_1_CRC_OK))
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;
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;
611 ao_telem_progress = (ao_telem_progress + 1) & 0x3;
613 case AO_TELEMETRY_LOCATION:
614 memcpy(&ao_tel_location, &ao_monitor_ring[monitor], sizeof (ao_tel_location));
616 case AO_TELEMETRY_CONFIGURATION:
617 memcpy(&ao_tel_config, &ao_monitor_ring[monitor], sizeof (ao_tel_config));
622 static struct ao_task ao_terramonitor_task;
627 uint16_t gps_tick = ao_gps_progress;
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;
637 static struct ao_task ao_terragps_task;
640 ao_terraui_init(void)
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");