X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fproduct%2Fao_terraui.c;h=f8f23a0ff00771b649de8798725dd32c9031bbe9;hp=cfbfb1fb8b3b8ad461927bc6232d96bd6a52caca;hb=6023ff81f1bbd240169b9548209133d3b02d475f;hpb=da330c5975b9f565d059ef8084dfdacc20f34246 diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c index cfbfb1fb..f8f23a0f 100644 --- a/src/product/ao_terraui.c +++ b/src/product/ao_terraui.c @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -16,6 +17,7 @@ */ #include "ao.h" +#include #include static __xdata struct ao_telemetry_sensor ao_tel_sensor; @@ -89,13 +91,28 @@ static uint8_t ao_telem_progress; static uint8_t ao_gps_progress; static void -ao_terraui_info(void) +ao_terraui_startup(void) +{ + sprintf(ao_lcd_line, "%-16.16s", ao_product); + ao_terraui_line(AO_LCD_ADDR(0,0)); + sprintf(ao_lcd_line, "%-8.8s %5u ", ao_version, ao_serial_number); + ao_terraui_line(AO_LCD_ADDR(1,0)); +} + +static void +ao_terraui_info_firstline(void) { sprintf(ao_lcd_line, "S %4d %7.7s %c", ao_tel_sensor.serial, ao_tel_config.callsign, ao_terraui_state()); ao_terraui_line(AO_LCD_ADDR(0,0)); +} + +static void +ao_terraui_info(void) +{ + ao_terraui_info_firstline(); sprintf(ao_lcd_line, "F %4d RSSI%4d%c", ao_tel_config.flight, ao_tel_rssi, @@ -145,20 +162,26 @@ static int16_t mag(int32_t d) return d; } -static uint32_t +static int32_t dist(int32_t d) { - int32_t m; + __pdata uint32_t m; + uint8_t neg = 0; - if (d < 0) + if (d < 0) { d = -d; + neg = 1; + } - m = 1000000; + m = 10000000; while (d >= (2147483647 / 111198)) { d /= 10; m /= 10; } - return (d * 111198) / m; + d = (d * 111198) / m; + if (neg) + d = -d; + return d; } static __code uint8_t cos_table[] = { @@ -295,22 +318,25 @@ int16_t ao_atan2(int32_t dy, int32_t dx) __reentrant } if (dy > dx) { - int t; + int32_t t; t = dy; dy = dx; dx = t; a = a + m * 90; m = -m; } - r = ((dy << 8) + (dx >> 1)) / dx; + dy = dy << 8; + dy = (dy + (dx >> 1)) / dx; + r = dy; for (t = 0; t < 44; t++) if (tan_table[t] >= r) break; return t * m + a; } -static __pdata uint32_t lon_dist, lat_dist; +static __pdata int32_t lon_dist, lat_dist; static __pdata uint32_t ground_dist, range; +static __pdata uint8_t dist_in_km; static __pdata int16_t bearing, elevation; static void @@ -320,22 +346,45 @@ ao_terraui_lat_dist(void) } static void -ao_terraui_lon_dist(void) +ao_terraui_lon_dist(void) __reentrant { uint8_t c = cos_table[ao_gps_data.latitude >> 24]; - lon_dist = dist(ao_tel_location.longitude - ao_gps_data.longitude); - if (c) - lon_dist = ((uint32_t) lon_dist * c) >> 8; + lon_dist = ao_tel_location.longitude; + + /* check if it's shorter to go the other way around */ + if ((int16_t) (lon_dist >> 24) < (int16_t) (ao_gps_data.longitude >> 24) - (int16_t) (1800000000 >> 24)) + lon_dist += 3600000000ul; + lon_dist = dist(lon_dist - ao_gps_data.longitude); + if (c) { + if (lon_dist & 0x7f800000) + lon_dist = (lon_dist >> 8) * c; + else + lon_dist = (lon_dist * (int16_t) c) >> 8; + } } +static int32_t sqr(int32_t x) { return x * x; } + static void ao_terraui_compute(void) { + uint16_t h = ao_tel_sensor.height; + ao_terraui_lat_dist(); ao_terraui_lon_dist(); - ground_dist = ao_sqrt (lat_dist * lat_dist + lon_dist * lon_dist); + if (lat_dist > 32767 || lon_dist > 32767) { + dist_in_km = 1; + ground_dist = sqr(lat_dist/1000) + sqr(lon_dist/1000); + h = h/1000; + } else { + dist_in_km = 0; + ground_dist = sqr(lat_dist) + sqr(lon_dist); + } + ground_dist = ao_sqrt (ground_dist); range = ao_sqrt(ground_dist * ground_dist + ao_tel_sensor.height * ao_tel_sensor.height); - bearing = ao_atan2(lat_dist, lon_dist); + bearing = ao_atan2(lon_dist, lat_dist); + if (bearing < 0) + bearing += 360; elevation = ao_atan2(ao_tel_sensor.height, ground_dist); } @@ -353,6 +402,21 @@ ao_terraui_descent(void) ao_terraui_line(AO_LCD_ADDR(1,0)); } +static void +ao_terraui_recover(void) +{ + ao_terraui_compute(); + sprintf(ao_lcd_line, "\007 %4d \005 %3d %c", + bearing, elevation, + ao_terraui_state()); + ao_terraui_line(AO_LCD_ADDR(0,0)); + sprintf(ao_lcd_line, "R%5ld%cRSSI%4d%c", + ground_dist, dist_in_km ? 'k' : ' ', + ao_tel_rssi, + ao_progress[ao_telem_progress]); + ao_terraui_line(AO_LCD_ADDR(1,0)); +} + static void ao_terraui_coord(int32_t c, char plus, char minus, char extra) __reentrant { @@ -393,73 +457,119 @@ ao_terraui_local(void) __reentrant ao_terraui_line(AO_LCD_ADDR(1,0)); } +static __pdata uint8_t ao_set_freq; +static __pdata uint32_t ao_set_freq_orig; + static void -ao_terraui_config(void) +ao_terraui_freq(void) __reentrant { + uint16_t MHz; + uint16_t frac; + MHz = ao_config.frequency / 1000; + frac = ao_config.frequency % 1000; + ao_terraui_info_firstline(); + sprintf(ao_lcd_line, "Freq: %3d.%03d MHz", MHz, frac); + ao_terraui_line(AO_LCD_ADDR(1,0)); + ao_lcd_goto(AO_LCD_ADDR(1,11)); +} + +static void +ao_terraui_freq_start(void) +{ + ao_set_freq = 1; + ao_set_freq_orig = ao_config.frequency; + ao_lcd_cursor_on(); } -enum ao_page { - ao_page_info, - ao_page_pad, - ao_page_ascent, - ao_page_descent, - ao_page_remote, - ao_page_local, +static void +ao_terraui_freq_button(char b) { + + switch (b) { + case 0: + return; + case 1: + if (ao_config.frequency > 430000) + ao_config.frequency -= 100; + break; + case 2: + ao_set_freq = 0; + ao_lcd_cursor_off(); + if (ao_set_freq_orig != ao_config.frequency) + ao_config_put(); + return; + case 3: + if (ao_config.frequency < 438000) + ao_config.frequency += 100; + break; + + } + ao_config_set_radio(); + ao_radio_recv_abort(); +} + +static __code void (*__code ao_terraui_page[])(void) = { + ao_terraui_startup, + ao_terraui_info, + ao_terraui_pad, + ao_terraui_ascent, + ao_terraui_descent, + ao_terraui_recover, + ao_terraui_remote, + ao_terraui_local, }; +#define NUM_PAGE (sizeof (ao_terraui_page)/sizeof (ao_terraui_page[0])) + +static __pdata uint8_t ao_current_page = 0; +static __pdata uint8_t ao_shown_about = 3; + static void ao_terraui(void) { - enum ao_page cur_page = ao_page_info; - ao_lcd_start(); + + ao_delay(AO_MS_TO_TICKS(100)); + ao_button_clear(); + for (;;) { char b; - switch (cur_page) { - case ao_page_info: - ao_terraui_info(); - break; - case ao_page_pad: - ao_terraui_pad(); - break; - case ao_page_ascent: - ao_terraui_ascent(); - break; - case ao_page_descent: - ao_terraui_descent(); - break; - case ao_page_remote: - ao_terraui_remote(); - break; - case ao_page_local: - ao_terraui_local(); - break; - } - ao_alarm(AO_SEC_TO_TICKS(1)); - b = ao_button_get(); - ao_clear_alarm(); + if (ao_set_freq) + ao_terraui_freq(); + else + ao_terraui_page[ao_current_page](); + b = ao_button_get(AO_SEC_TO_TICKS(1)); + + if (b > 0) { + ao_beep_for(AO_BEEP_HIGH, AO_MS_TO_TICKS(10)); + ao_shown_about = 0; + } + + if (ao_set_freq) { + ao_terraui_freq_button(b); + continue; + } switch (b) { case 0: + if (ao_shown_about) { + if (--ao_shown_about == 0) + ao_current_page = 2; + } break; case 1: - if (cur_page == ao_page_local) - cur_page = ao_page_info; - else - cur_page++; - ao_beep_for(AO_BEEP_HIGH, AO_MS_TO_TICKS(50)); + ao_current_page++; + if (ao_current_page >= NUM_PAGE) + ao_current_page = 0; break; case 2: - ao_beep_for(AO_BEEP_LOW, AO_MS_TO_TICKS(200)); + ao_terraui_freq_start(); break; case 3: - if (cur_page == ao_page_info) - cur_page = ao_page_local; - else - cur_page--; - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(50)); + if (ao_current_page == 0) + ao_current_page = NUM_PAGE; + ao_current_page--; break; } } @@ -473,16 +583,17 @@ ao_terramonitor(void) uint8_t monitor; monitor = ao_monitor_head; + ao_monitor_set(sizeof (struct ao_telemetry_generic)); for (monitor = ao_monitor_head;; monitor = ao_monitor_ring_next(monitor)) { while (monitor == ao_monitor_head) - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + ao_sleep(&ao_monitor_head); if (ao_monitoring != sizeof (union ao_telemetry_all)) continue; if (!(ao_monitor_ring[monitor].all.status & PKT_APPEND_STATUS_1_CRC_OK)) continue; - ao_tel_rssi = (ao_monitor_ring[monitor].all.rssi >> 1) - 74; + ao_tel_rssi = AO_RSSI_FROM_RADIO(ao_monitor_ring[monitor].all.rssi); switch (ao_monitor_ring[monitor].all.telemetry.generic.type) { case AO_TELEMETRY_SENSOR_TELEMETRUM: case AO_TELEMETRY_SENSOR_TELEMINI: @@ -517,7 +628,7 @@ ao_terragps(void) for (;;) { while (ao_gps_tick == gps_tick) - ao_sleep(&ao_gps_data); + ao_sleep(&ao_gps_new); gps_tick = ao_gps_tick; ao_gps_progress = (ao_gps_progress + 1) & 3; }