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; version 2 of the License.
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.
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.
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;
28 static __xdata char ao_lcd_line[17];
29 static __xdata char ao_state_name[] = "SIPBFCDMLI";
32 ao_terraui_line(uint8_t addr)
35 ao_lcd_putstring(ao_lcd_line);
38 #define ao_terraui_state() (ao_state_name[ao_tel_sensor.state])
41 ao_terraui_igniter(int16_t sense)
43 if (sense < AO_IGNITER_OPEN)
45 if (sense > AO_IGNITER_CLOSED)
51 ao_terraui_battery(void)
53 if (ao_tel_sensor.v_batt > 25558)
61 if (ao_tel_location.flags & (1 << 4)) {
62 if ((ao_tel_location.flags & 0xf) >= 4)
69 ao_terraui_local_gps(void)
71 if (ao_gps_data.flags & (1 << 4)) {
72 if ((ao_gps_data.flags & 0xf) >= 4)
79 ao_terraui_logging(void)
81 if (ao_tel_config.flight != 0)
86 static __code char ao_progress[4] = { '\011', '\012', '\014', '\013' };
88 static uint8_t ao_telem_progress;
89 static uint8_t ao_gps_progress;
94 sprintf(ao_lcd_line, "S %4d %7.7s %c",
96 ao_tel_config.callsign,
98 ao_terraui_line(AO_LCD_ADDR(0,0));
99 sprintf(ao_lcd_line, "F %4d RSSI%4d%c",
100 ao_tel_config.flight,
102 ao_progress[ao_telem_progress]);
103 ao_terraui_line(AO_LCD_ADDR(1,0));
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(),
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,
120 ao_progress[ao_telem_progress]);
121 ao_terraui_line(AO_LCD_ADDR(1,0));
125 ao_terraui_ascent(void)
127 sprintf(ao_lcd_line, "S %5d S\011%5d%c",
128 ao_tel_sensor.speed >> 4,
129 ao_tel_max_speed >> 4,
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));
139 static int16_t mag(int32_t d)
157 while (d >= (2147483647 / 111198)) {
161 return (d * 111198) / m;
164 static __code uint8_t cos_table[] = {
222 static __code uint8_t tan_table[] = {
270 int16_t ao_atan2(int32_t dy, int32_t dx) __reentrant
300 t = dy; dy = dx; dx = t;
305 r = ((dy << 8) + (dx >> 1)) / dx;
306 for (t = 0; t < 44; t++)
307 if (tan_table[t] >= r)
312 static __pdata uint32_t lon_dist, lat_dist;
313 static __pdata uint32_t ground_dist, range;
314 static __pdata int16_t bearing, elevation;
317 ao_terraui_lat_dist(void)
319 lat_dist = dist (ao_tel_location.latitude - ao_gps_data.latitude);
323 ao_terraui_lon_dist(void)
325 uint8_t c = cos_table[ao_gps_data.latitude >> 24];
326 lon_dist = dist(ao_tel_location.longitude - ao_gps_data.longitude);
328 lon_dist = ((uint32_t) lon_dist * c) >> 8;
332 ao_terraui_compute(void)
334 ao_terraui_lat_dist();
335 ao_terraui_lon_dist();
336 ground_dist = ao_sqrt (lat_dist * lat_dist + lon_dist * lon_dist);
337 range = ao_sqrt(ground_dist * ground_dist + ao_tel_sensor.height * ao_tel_sensor.height);
338 bearing = ao_atan2(lat_dist, lon_dist);
339 elevation = ao_atan2(ao_tel_sensor.height, ground_dist);
343 ao_terraui_descent(void)
345 ao_terraui_compute();
346 sprintf(ao_lcd_line, "\007 %4d \005 %3d %c",
349 ao_terraui_line(AO_LCD_ADDR(0,0));
350 sprintf(ao_lcd_line, "H:%5d S:%5d%c",
351 ao_tel_sensor.height, ao_tel_sensor.speed >> 4,
352 ao_progress[ao_telem_progress]);
353 ao_terraui_line(AO_LCD_ADDR(1,0));
357 ao_terraui_coord(int32_t c, char plus, char minus, char extra) __reentrant
372 f = (c + 500) / 1000;
373 sprintf(ao_lcd_line, "%c %3d\362 %2d.%04d\"%c",
374 plus, d, m, f, extra);
378 ao_terraui_remote(void)
380 ao_terraui_coord(ao_tel_location.latitude, 'N', 'S', ao_terraui_state());
381 ao_terraui_line(AO_LCD_ADDR(0,0));
382 ao_terraui_coord(ao_tel_location.longitude, 'E', 'W', ao_progress[ao_telem_progress]);
383 ao_terraui_line(AO_LCD_ADDR(1,0));
387 ao_terraui_local(void) __reentrant
389 ao_terraui_coord(ao_gps_data.latitude, 'n', 's',
390 ao_terraui_local_gps());
391 ao_terraui_line(AO_LCD_ADDR(0,0));
392 ao_terraui_coord(ao_gps_data.longitude, 'e', 'w', ao_progress[ao_gps_progress]);
393 ao_terraui_line(AO_LCD_ADDR(1,0));
397 ao_terraui_config(void)
414 enum ao_page cur_page = ao_page_info;
429 case ao_page_descent:
430 ao_terraui_descent();
440 ao_alarm(AO_SEC_TO_TICKS(1));
448 if (cur_page == ao_page_local)
449 cur_page = ao_page_info;
452 ao_beep_for(AO_BEEP_HIGH, AO_MS_TO_TICKS(50));
455 ao_beep_for(AO_BEEP_LOW, AO_MS_TO_TICKS(200));
458 if (cur_page == ao_page_info)
459 cur_page = ao_page_local;
462 ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(50));
468 __xdata static struct ao_task ao_terraui_task;
471 ao_terramonitor(void)
475 monitor = ao_monitor_head;
476 for (monitor = ao_monitor_head;;
477 monitor = ao_monitor_ring_next(monitor))
479 while (monitor == ao_monitor_head)
480 ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
481 if (ao_monitoring != sizeof (union ao_telemetry_all))
483 if (!(ao_monitor_ring[monitor].all.status & PKT_APPEND_STATUS_1_CRC_OK))
485 ao_tel_rssi = (ao_monitor_ring[monitor].all.rssi >> 1) - 74;
486 switch (ao_monitor_ring[monitor].all.telemetry.generic.type) {
487 case AO_TELEMETRY_SENSOR_TELEMETRUM:
488 case AO_TELEMETRY_SENSOR_TELEMINI:
489 case AO_TELEMETRY_SENSOR_TELENANO:
490 ao_xmemcpy(&ao_tel_sensor, &ao_monitor_ring[monitor], sizeof (ao_tel_sensor));
491 if (ao_tel_sensor.state < ao_flight_boost) {
492 ao_tel_max_speed = 0;
493 ao_tel_max_height = 0;
495 if (ao_tel_sensor.speed > ao_tel_max_speed)
496 ao_tel_max_speed = ao_tel_sensor.speed;
497 if (ao_tel_sensor.height > ao_tel_max_height)
498 ao_tel_max_height = ao_tel_sensor.height;
500 ao_telem_progress = (ao_telem_progress + 1) & 0x3;
502 case AO_TELEMETRY_LOCATION:
503 ao_xmemcpy(&ao_tel_location, &ao_monitor_ring[monitor], sizeof (ao_tel_location));
505 case AO_TELEMETRY_CONFIGURATION:
506 ao_xmemcpy(&ao_tel_config, &ao_monitor_ring[monitor], sizeof (ao_tel_config));
511 __xdata static struct ao_task ao_terramonitor_task;
516 uint16_t gps_tick = ao_gps_progress;
519 while (ao_gps_tick == gps_tick)
520 ao_sleep(&ao_gps_data);
521 gps_tick = ao_gps_tick;
522 ao_gps_progress = (ao_gps_progress + 1) & 3;
526 __xdata static struct ao_task ao_terragps_task;
529 ao_terraui_init(void)
531 ao_add_task(&ao_terraui_task, ao_terraui, "ui");
532 ao_add_task(&ao_terramonitor_task, ao_terramonitor, "monitor");
533 ao_add_task(&ao_terragps_task, ao_terragps, "gps");