2 * Copyright © 2014 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.
19 #include <ao_fake_flight.h>
20 #if HAS_MS5607 || HAS_MS5611
21 #include <ao_ms5607.h>
24 uint8_t ao_fake_flight_active;
26 static uint8_t ao_fake_has_cur;
27 static volatile uint8_t ao_fake_has_next;
28 static uint8_t ao_fake_has_offset;
29 static uint16_t ao_fake_tick_offset;
30 static struct ao_data ao_fake_cur, ao_fake_next;
33 ao_fake_flight_poll(void)
35 if (ao_fake_has_next && (ao_tick_count - ao_fake_next.tick) >= 0) {
36 ao_fake_cur = ao_fake_next;
38 ao_wakeup((void *) &ao_fake_has_next);
43 ao_data_ring[ao_data_head] = ao_fake_cur;
44 ao_data_ring[ao_data_head].tick = ao_tick_count;
45 ao_data_head = ao_data_ring_next(ao_data_head);
46 ao_wakeup((void *) &ao_data_head);
50 ao_fake_data_read(void)
53 uint8_t *d = (void *) &ao_fake_next;
57 for (i = 0; i < sizeof (struct ao_data); i++)
59 if (!ao_fake_has_offset) {
60 ao_fake_tick_offset = (ao_tick_count + 1000) - ao_fake_next.tick;
61 ao_fake_next.tick = ao_tick_count;
62 ao_fake_has_offset = 1;
64 ao_fake_next.tick += ao_fake_tick_offset;
70 ao_fake_calib_get(struct ao_fake_calib *calib)
73 calib->accel_plus_g = ao_config.accel_plus_g;
74 calib->accel_minus_g = ao_config.accel_minus_g;
77 calib->accel_zero_along = ao_config.accel_zero_along;
78 calib->accel_zero_across = ao_config.accel_zero_across;
79 calib->accel_zero_through = ao_config.accel_zero_through;
81 #if HAS_MS5607 || HAS_MS5611
82 calib->ms5607_prom = ao_ms5607_prom;
87 ao_fake_calib_set(struct ao_fake_calib *calib)
90 ao_config.accel_plus_g = calib->accel_plus_g;
91 ao_config.accel_minus_g = calib->accel_minus_g;
94 ao_config.accel_zero_along = calib->accel_zero_along;
95 ao_config.accel_zero_across = calib->accel_zero_across;
96 ao_config.accel_zero_through = calib->accel_zero_through;
98 #if HAS_MS5607 || HAS_MS5611
99 ao_ms5607_prom = calib->ms5607_prom;
104 ao_fake_calib_read(void)
106 struct ao_fake_calib ao_calib;
107 uint8_t *d = (void *) &ao_calib;
110 /* Read calibration data */
111 for (i = 0; i < sizeof (struct ao_fake_calib); i++)
113 if (ao_calib.major != AO_FAKE_CALIB_MAJOR
114 #if AO_FAKE_CALIB_MINOR != 0
115 || ao_calib.minor < AO_FAKE_CALIB_MINOR
118 printf ("Calibration data major version mismatch %d.%d <= %d.%d\n",
119 ao_calib.major, ao_calib.minor, AO_FAKE_CALIB_MAJOR, AO_FAKE_CALIB_MINOR);
122 ao_fake_calib_set(&ao_calib);
129 int16_t calib_size, data_size;
130 struct ao_fake_calib save_calib;
131 uint16_t my_pyro_fired = 0;
132 enum ao_flight_state my_state = ao_flight_invalid;
136 if (ao_cmd_status != ao_cmd_success)
138 calib_size = ao_cmd_lex_i;
140 if (ao_cmd_status != ao_cmd_success)
142 data_size = ao_cmd_lex_i;
143 if ((unsigned) calib_size != sizeof (struct ao_fake_calib)) {
144 printf ("calib size %d larger than actual size %d\n",
145 calib_size, sizeof (struct ao_fake_calib));
146 ao_cmd_status = ao_cmd_syntax_error;
149 if (data_size != sizeof (struct ao_data)) {
150 printf ("data size %d doesn't match actual size %d\n",
151 data_size, sizeof (struct ao_data));
152 ao_cmd_status = ao_cmd_syntax_error;
155 ao_fake_calib_get(&save_calib);
156 if (!ao_fake_calib_read())
159 ao_fake_has_next = 0;
161 ao_fake_flight_active = 1;
164 ao_packet_slave_stop();
167 /* Turn on the LED to indicate startup */
168 ao_led_on(AO_LED_RED);
170 ao_flight_state = ao_flight_startup;
172 if (my_state != ao_flight_state) {
173 printf("state %d\n", ao_flight_state);
174 my_state = ao_flight_state;
177 if (my_pyro_fired != ao_pyro_fired) {
180 for (pyro = 0; pyro < AO_PYRO_NUM; pyro++) {
181 uint16_t bit = (1 << pyro);
182 if (!(my_pyro_fired & bit) && (ao_pyro_fired & bit))
183 printf ("fire %d\n", pyro);
185 my_pyro_fired = ao_pyro_fired;
187 while (ao_fake_has_next)
188 ao_sleep((void *) &ao_fake_has_next);
189 if (!ao_fake_data_read())
193 /* Wait 20 seconds to see if we enter landed state */
194 for (i = 0; i < 200; i++)
196 if (ao_flight_state == ao_flight_landed)
198 ao_delay(AO_MS_TO_TICKS(100));
201 /* Turn on the LED to indicate startup */
202 ao_led_on(AO_LED_RED);
204 ao_fake_flight_active = 0;
205 ao_flight_state = ao_flight_startup;
207 ao_fake_calib_set(&save_calib);
210 static const struct ao_cmds ao_fake_flight_cmds[] = {
211 { ao_fake_flight, "F <calib-size> <data-size>\0Start fake flight" },
216 ao_fake_flight_init(void)
218 ao_cmd_register(&ao_fake_flight_cmds[0]);