2 * Copyright © 2022 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.
19 #ifndef AO_FLIGHT_TEST
24 #include <ao_flight.h>
26 /* Main flight thread. */
28 enum ao_flight_state ao_flight_state; /* current flight state */
29 AO_TICK_TYPE ao_launch_tick; /* time of first boost detect */
32 /* Any sensor can set this to mark the flight computer as 'broken' */
33 uint8_t ao_sensor_errors;
37 * track min/max data over a long interval to detect
40 static AO_TICK_TYPE ao_interval_end;
42 #define init_bounds(_cur, _min, _max) do { \
46 #define check_bounds(_cur, _min, _max) do { \
53 uint8_t ao_flight_force_idle;
56 * Landing is detected by getting constant readings from pressure sensor
57 * for a fairly long time (AO_INTERVAL_TICKS), along with the max being
58 * less than the boost detect pressure
60 #define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10)
62 static AO_TICK_TYPE ao_interval_end;
63 static motor_pressure_t ao_interval_min_motor_pressure, ao_interval_max_motor_pressure;
65 #define abs(a) ((a) < 0 ? -(a) : (a))
71 ao_flight_state = ao_flight_startup;
75 * Process ADC samples, just looping
76 * until the sensors are calibrated.
81 switch (ao_flight_state) {
82 case ao_flight_startup:
84 if (!ao_flight_force_idle)
86 /* Set pad mode - we can fly! */
87 ao_flight_state = ao_flight_pad;
88 #if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE && !DEBUG
89 /* Disable the USB controller in flight mode
93 if (!ao_fake_flight_active)
99 /* signal successful initialization by turning off the LED */
100 ao_led_off(AO_LED_RED);
104 ao_flight_state = ao_flight_idle;
105 #if HAS_SENSOR_ERRORS
106 if (ao_sensor_errors)
107 ao_flight_state = ao_flight_invalid;
111 /* signal successful initialization by turning off the LED */
112 ao_led_off(AO_LED_RED);
115 /* wakeup threads due to state change */
116 ao_wakeup(&ao_flight_state);
123 * motor pressure rise > 50psi
125 if (ao_sample_motor_pressure - ao_ground_motor_pressure >= AO_BOOST_DETECT)
127 ao_flight_state = ao_flight_boost;
128 ao_wakeup(&ao_flight_state);
130 ao_launch_tick = ao_sample_tick;
132 /* start logging data */
136 /* Initialize landing detection interval values */
137 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
139 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
142 case ao_flight_boost:
145 * motor pressure low and stable for more than 10 seconds
147 check_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure,
148 ao_interval_max_motor_pressure);
150 if ((AO_TICK_SIGNED) (ao_sample_tick - ao_interval_end) >= 0) {
151 if (ao_interval_max_motor_pressure - ao_ground_motor_pressure <= AO_BOOST_DETECT &&
152 ao_interval_max_motor_pressure - ao_interval_min_motor_pressure <= AO_QUIET_DETECT_PRESSURE)
154 ao_flight_state = ao_flight_landed;
155 ao_wakeup(&ao_flight_state);
157 /* turn off the ADC capture */
158 ao_timer_set_adc_interval(0);
162 /* Reset interval values */
163 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
165 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
175 static inline int int_part(ao_v_t i) { return i >> 4; }
176 static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; }
184 accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
187 printf ("sample:\n");
188 printf (" tick %d\n", ao_sample_tick);
190 printf (" raw pres %ld\n", ao_sample_pres);
193 printf (" raw accel %d\n", ao_sample_accel);
196 printf (" ground pres %ld\n", ao_ground_pres);
197 printf (" ground alt %ld\n", ao_ground_height);
200 printf (" raw accel %d\n", ao_sample_accel);
201 printf (" groundaccel %d\n", ao_ground_accel);
202 printf (" accel_2g %d\n", ao_accel_2g);
206 printf (" alt %ld\n", ao_sample_alt);
207 printf (" height %ld\n", ao_sample_height);
211 printf (" accel %d.%02d\n", int_part(accel), frac_part(accel));
215 printf ("kalman:\n");
216 printf (" height %ld\n", ao_height);
217 printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
218 printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
219 printf (" max_height %ld\n", ao_max_height);
220 printf (" avg_height %ld\n", ao_avg_height);
221 printf (" error_h %ld\n", ao_error_h);
223 printf (" error_avg %d\n", ao_error_h_sq_avg);
230 ao_flight_state = ao_flight_test;
232 ao_flight_state = ao_flight_idle;
235 uint8_t ao_orient_test;
238 ao_orient_test_select(void)
240 ao_orient_test = !ao_orient_test;
241 printf("orient test %d\n", ao_orient_test);
244 const struct ao_cmds ao_flight_cmds[] = {
245 { ao_flight_dump, "F\0Dump flight status" },
246 { ao_gyro_test, "G\0Test gyro code" },
247 { ao_orient_test_select,"O\0Test orientation code" },
252 static struct ao_task flight_task;
257 ao_flight_state = ao_flight_startup;
259 ao_cmd_register(&ao_flight_cmds[0]);
261 ao_add_task(&flight_task, ao_flight, "flight");