Update flight algorithm based on data collected from SN-1 Flight 1
[fw/altos] / ao_monitor.c
1 /*
2  * Copyright © 2009 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
20 const char const * const ao_state_names[] = {
21         "startup", "idle", "pad", "boost", "coast",
22         "apogee", "drogue", "main", "landed", "invalid"
23 };
24
25 __xdata uint8_t ao_monitoring;
26
27 void
28 ao_monitor(void)
29 {
30         __xdata struct ao_radio_recv recv;
31         uint8_t state;
32
33         for (;;) {
34                 __critical while (!ao_monitoring)
35                         ao_sleep(&ao_monitoring);
36                 ao_radio_recv(&recv);
37                 state = recv.telemetry.flight_state;
38                 if (state > ao_flight_invalid)
39                         state = ao_flight_invalid;
40                 printf ("SERIAL %3d RSSI %3d STATUS %02x STATE %s ",
41                         recv.telemetry.addr, recv.rssi, recv.status,
42                         ao_state_names[state]);
43                 if (!(recv.status & PKT_APPEND_STATUS_1_CRC_OK))
44                         printf("CRC INVALID ");
45                 printf("%5u a: %d p: %d t: %d v: %d d: %d m: %d ",
46                        recv.telemetry.adc.tick,
47                        recv.telemetry.adc.accel,
48                        recv.telemetry.adc.pres,
49                        recv.telemetry.adc.temp,
50                        recv.telemetry.adc.v_batt,
51                        recv.telemetry.adc.sense_d,
52                        recv.telemetry.adc.sense_m);
53                 ao_gps_print(&recv.telemetry.gps);
54                 ao_usb_flush();
55                 ao_led_for(AO_LED_GREEN, AO_MS_TO_TICKS(10));
56         }
57 }
58
59 __xdata struct ao_task ao_monitor_task;
60
61 static void
62 ao_set_monitor(void)
63 {
64         ao_cmd_hex();
65         ao_monitoring = ao_cmd_lex_i != 0;
66         ao_wakeup(&ao_monitoring);
67 }
68
69 __code struct ao_cmds ao_monitor_cmds[] = {
70         { 'M',  ao_set_monitor, "M                                  Enable/disable radio monitoring" },
71         { 0,    ao_set_monitor, NULL },
72 };
73
74 void
75 ao_monitor_init(void)
76 {
77         ao_monitoring = 0;
78         ao_cmd_register(&ao_monitor_cmds[0]);
79         ao_add_task(&ao_monitor_task, ao_monitor, "monitor");
80 }