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;
55 /* Compute ADC value change given a defined pressure change in Pa */
58 ao_delta_pressure_to_adc(uint32_t pressure)
60 static const double volts_base = AO_PRESSURE_VOLTS_BASE;
61 static const double volts_max = AO_PRESSURE_VOLTS_MAX;
63 /* Compute change in voltage from the sensor */
64 double volts = (double) pressure / AO_FULL_SCALE_PRESSURE * (volts_max - volts_base);
66 /* voltage divider in front of the ADC input to decivolts */
67 double adc_dv = volts * (10.0 * (double) AO_PRESSURE_DIV_MINUS /
68 ((double) AO_PRESSURE_DIV_PLUS + (double) AO_PRESSURE_DIV_MINUS));
70 /* convert to ADC output value */
71 double adc = adc_dv * AO_ADC_MAX / AO_ADC_REFERENCE_DV;
81 #define AO_BOOST_DETECT ao_delta_pressure_to_adc(AO_BOOST_DETECT_PRESSURE)
82 #define AO_QUIET_DETECT ao_delta_pressure_to_adc(AO_QUIET_DETECT_PRESSURE)
85 * Landing is detected by getting constant readings from pressure sensor
86 * for a fairly long time (AO_INTERVAL_TICKS), along with the max being
87 * less than the boost detect pressure
89 #define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10)
91 static AO_TICK_TYPE ao_interval_end;
92 static motor_pressure_t ao_interval_min_motor_pressure, ao_interval_max_motor_pressure;
94 #define abs(a) ((a) < 0 ? -(a) : (a))
100 ao_flight_state = ao_flight_startup;
104 * Process ADC samples, just looping
105 * until the sensors are calibrated.
110 switch (ao_flight_state) {
111 case ao_flight_startup:
113 if (!ao_flight_force_idle)
115 /* Set pad mode - we can fly! */
116 ao_flight_state = ao_flight_pad;
117 #if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE && !DEBUG
118 /* Disable the USB controller in flight mode
122 if (!ao_fake_flight_active)
128 /* signal successful initialization by turning off the LED */
129 ao_led_off(AO_LED_RED);
133 ao_flight_state = ao_flight_idle;
134 #if HAS_SENSOR_ERRORS
135 if (ao_sensor_errors)
136 ao_flight_state = ao_flight_invalid;
140 /* signal successful initialization by turning off the LED */
141 ao_led_off(AO_LED_RED);
144 /* wakeup threads due to state change */
145 ao_wakeup(&ao_flight_state);
152 * motor pressure rise > 50psi
154 if (ao_sample_motor_pressure - ao_ground_motor_pressure >= AO_BOOST_DETECT)
156 ao_flight_state = ao_flight_boost;
157 ao_wakeup(&ao_flight_state);
159 ao_launch_tick = ao_sample_tick;
161 /* start logging data */
165 /* Initialize landing detection interval values */
166 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
168 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
171 case ao_flight_boost:
174 * motor pressure low and stable for more than 10 seconds
176 check_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure,
177 ao_interval_max_motor_pressure);
179 if ((AO_TICK_SIGNED) (ao_sample_tick - ao_interval_end) >= 0) {
180 if (ao_interval_max_motor_pressure - ao_ground_motor_pressure <= AO_BOOST_DETECT &&
181 ao_interval_max_motor_pressure - ao_interval_min_motor_pressure <= AO_QUIET_DETECT_PRESSURE)
183 ao_flight_state = ao_flight_landed;
184 ao_wakeup(&ao_flight_state);
186 /* turn off the ADC capture */
187 ao_timer_set_adc_interval(0);
191 /* Reset interval values */
192 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
194 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
204 static inline int int_part(ao_v_t i) { return i >> 4; }
205 static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; }
213 accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
216 printf ("sample:\n");
217 printf (" tick %d\n", ao_sample_tick);
219 printf (" raw pres %ld\n", ao_sample_pres);
222 printf (" raw accel %d\n", ao_sample_accel);
225 printf (" ground pres %ld\n", ao_ground_pres);
226 printf (" ground alt %ld\n", ao_ground_height);
229 printf (" raw accel %d\n", ao_sample_accel);
230 printf (" groundaccel %d\n", ao_ground_accel);
231 printf (" accel_2g %d\n", ao_accel_2g);
235 printf (" alt %ld\n", ao_sample_alt);
236 printf (" height %ld\n", ao_sample_height);
240 printf (" accel %d.%02d\n", int_part(accel), frac_part(accel));
244 printf ("kalman:\n");
245 printf (" height %ld\n", ao_height);
246 printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
247 printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
248 printf (" max_height %ld\n", ao_max_height);
249 printf (" avg_height %ld\n", ao_avg_height);
250 printf (" error_h %ld\n", ao_error_h);
252 printf (" error_avg %d\n", ao_error_h_sq_avg);
259 ao_flight_state = ao_flight_test;
261 ao_flight_state = ao_flight_idle;
264 uint8_t ao_orient_test;
267 ao_orient_test_select(void)
269 ao_orient_test = !ao_orient_test;
270 printf("orient test %d\n", ao_orient_test);
273 const struct ao_cmds ao_flight_cmds[] = {
274 { ao_flight_dump, "F\0Dump flight status" },
275 { ao_gyro_test, "G\0Test gyro code" },
276 { ao_orient_test_select,"O\0Test orientation code" },
281 static struct ao_task flight_task;
286 ao_flight_state = ao_flight_startup;
288 ao_cmd_register(&ao_flight_cmds[0]);
290 ao_add_task(&flight_task, ao_flight, "flight");