From 1a467bf13485649419e1205ee788ef0d58b42d01 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 4 Dec 2022 16:32:05 -0800 Subject: [PATCH] easymotor-v3.0: Use motor pressure to trigger data logging Use this instead of acceleration so it can be used on the ground as well as for staged flights. Signed-off-by: Keith Packard --- src/easymotor-v3/Makefile | 3 +- src/easymotor-v3/ao_pins.h | 36 ++++- src/kernel/ao_motor_flight.c | 262 +++++++++++++++++++++++++++++++++++ src/kernel/ao_report.c | 4 + src/kernel/ao_sample.c | 10 +- 5 files changed, 307 insertions(+), 8 deletions(-) create mode 100644 src/kernel/ao_motor_flight.c diff --git a/src/easymotor-v3/Makefile b/src/easymotor-v3/Makefile index b33572a8..dc79ea24 100644 --- a/src/easymotor-v3/Makefile +++ b/src/easymotor-v3/Makefile @@ -28,8 +28,7 @@ ALTOS_SRC = \ ao_stdio.c \ ao_storage.c \ ao_report.c \ - ao_flight.c \ - ao_kalman.c \ + ao_motor_flight.c \ ao_sample.c \ ao_data.c \ ao_convert_volt.c \ diff --git a/src/easymotor-v3/ao_pins.h b/src/easymotor-v3/ao_pins.h index 856c7639..5cc95eb9 100644 --- a/src/easymotor-v3/ao_pins.h +++ b/src/easymotor-v3/ao_pins.h @@ -90,10 +90,6 @@ #define AO_DATA_RING 32 -struct ao_adc { - int16_t v_batt; - int16_t motor_pressure; -}; #define AO_ADC_DUMP(p) \ printf("tick: %5lu batt: %5d motor_pressure: %5d\n", \ @@ -152,10 +148,42 @@ struct ao_adc { #define HAS_IMU 1 #define USE_ADXL375_IMU 1 +#define HAS_KALMAN 0 + /* Motor pressure */ #define HAS_MOTOR_PRESSURE 1 #define ao_data_motor_pressure(packet) ((packet)->adc.motor_pressure) typedef int16_t motor_pressure_t; +/* want about 50psi, or 344kPa */ + +#define AO_FULL_SCALE_PRESSURE 11031612 /* 1600psi */ +#define AO_BOOST_DETECT_PRESSURE 344000 /* 50psi */ +#define AO_QUIET_DETECT_PRESSURE 207000 /* 30psi */ + +static inline int16_t ao_delta_pressure_to_adc(uint32_t pressure) +{ + static const double volts_base = 0.5; + static const double volts_max = 4.5; + + double volts = (double) pressure / AO_FULL_SCALE_PRESSURE * (volts_max - volts_base); + double adc_volts = volts * 10.0/15.6; /* voltage divider in front of the ADC input */ + if (adc_volts > 1.0) + adc_volts = 1.0; + double adc = adc_volts * 32767.0; + + if (adc < 0) + adc = 0; + return (int16_t) adc; +} + +#define AO_BOOST_DETECT ao_delta_pressure_to_adc(AO_BOOST_DETECT_PRESSURE) +#define AO_QUIET_DETECT ao_delta_pressure_to_adc(AO_QUIET_DETECT_PRESSURE) + +struct ao_adc { + int16_t v_batt; + int16_t motor_pressure; +}; + #endif /* _AO_PINS_H_ */ diff --git a/src/kernel/ao_motor_flight.c b/src/kernel/ao_motor_flight.c new file mode 100644 index 00000000..f47f59b3 --- /dev/null +++ b/src/kernel/ao_motor_flight.c @@ -0,0 +1,262 @@ +/* + * Copyright © 2022 Keith Packard + * + * 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; 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 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include +#endif + +#include + +/* Main flight thread. */ + +enum ao_flight_state ao_flight_state; /* current flight state */ +AO_TICK_TYPE ao_launch_tick; /* time of first boost detect */ + +#if HAS_SENSOR_ERRORS +/* Any sensor can set this to mark the flight computer as 'broken' */ +uint8_t ao_sensor_errors; +#endif + +/* + * track min/max data over a long interval to detect + * resting + */ +static AO_TICK_TYPE ao_interval_end; + +#define init_bounds(_cur, _min, _max) do { \ + _min = _max = _cur; \ + } while (0) + +#define check_bounds(_cur, _min, _max) do { \ + if (_cur < _min) \ + _min = _cur; \ + if (_cur > _max) \ + _max = _cur; \ + } while(0) + +uint8_t ao_flight_force_idle; + +/* + * Landing is detected by getting constant readings from pressure sensor + * for a fairly long time (AO_INTERVAL_TICKS), along with the max being + * less than the boost detect pressure + */ +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) + +static AO_TICK_TYPE ao_interval_end; +static motor_pressure_t ao_interval_min_motor_pressure, ao_interval_max_motor_pressure; + +#define abs(a) ((a) < 0 ? -(a) : (a)) + +void +ao_flight(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + for (;;) { + + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + + if (!ao_flight_force_idle) + { + /* Set pad mode - we can fly! */ + ao_flight_state = ao_flight_pad; +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE && !DEBUG + /* Disable the USB controller in flight mode + * to save power + */ +#if HAS_FAKE_FLIGHT + if (!ao_fake_flight_active) +#endif + ao_usb_disable(); +#endif + +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } else { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; +#if HAS_SENSOR_ERRORS + if (ao_sensor_errors) + ao_flight_state = ao_flight_invalid; +#endif + +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } + /* wakeup threads due to state change */ + ao_wakeup(&ao_flight_state); + + break; + + case ao_flight_pad: + /* pad to boost: + * + * motor pressure rise > 50psi + */ + if (ao_sample_motor_pressure - ao_ground_motor_pressure >= AO_BOOST_DETECT) + { + ao_flight_state = ao_flight_boost; + ao_wakeup(&ao_flight_state); + + ao_launch_tick = ao_sample_tick; + + /* start logging data */ +#if HAS_LOG + ao_log_start(); +#endif + /* Initialize landing detection interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure); + } + break; + case ao_flight_boost: + /* boost to landed: + * + * motor pressure low and stable for more than 10 seconds + */ + check_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, + ao_interval_max_motor_pressure); + + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_motor_pressure - ao_ground_motor_pressure <= AO_BOOST_DETECT && + ao_interval_max_motor_pressure - ao_interval_min_motor_pressure <= AO_QUIET_DETECT_PRESSURE) + { + ao_flight_state = ao_flight_landed; + ao_wakeup(&ao_flight_state); +#if HAS_ADC + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); +#endif + } + + /* Reset interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure); + } + break; + default: + break; + } + } +} + +#if HAS_FLIGHT_DEBUG +static inline int int_part(ao_v_t i) { return i >> 4; } +static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; } + +static void +ao_flight_dump(void) +{ +#if HAS_ACCEL + ao_v_t accel; + + accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; +#endif + + printf ("sample:\n"); + printf (" tick %d\n", ao_sample_tick); +#if HAS_BARO + printf (" raw pres %ld\n", ao_sample_pres); +#endif +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); +#endif +#if HAS_BARO + printf (" ground pres %ld\n", ao_ground_pres); + printf (" ground alt %ld\n", ao_ground_height); +#endif +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); + printf (" groundaccel %d\n", ao_ground_accel); + printf (" accel_2g %d\n", ao_accel_2g); +#endif + +#if HAS_BARO + printf (" alt %ld\n", ao_sample_alt); + printf (" height %ld\n", ao_sample_height); +#endif + +#if HAS_ACCEL + printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); +#endif + + + printf ("kalman:\n"); + printf (" height %ld\n", ao_height); + printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed)); + printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel)); + printf (" max_height %ld\n", ao_max_height); + printf (" avg_height %ld\n", ao_avg_height); + printf (" error_h %ld\n", ao_error_h); +#if !HAS_ACCEL + printf (" error_avg %d\n", ao_error_h_sq_avg); +#endif +} + +static void +ao_gyro_test(void) +{ + ao_flight_state = ao_flight_test; + ao_getchar(); + ao_flight_state = ao_flight_idle; +} + +uint8_t ao_orient_test; + +static void +ao_orient_test_select(void) +{ + ao_orient_test = !ao_orient_test; + printf("orient test %d\n", ao_orient_test); +} + +const struct ao_cmds ao_flight_cmds[] = { + { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, + { ao_orient_test_select,"O\0Test orientation code" }, + { 0, NULL }, +}; +#endif + +static struct ao_task flight_task; + +void +ao_flight_init(void) +{ + ao_flight_state = ao_flight_startup; +#if HAS_FLIGHT_DEBUG + ao_cmd_register(&ao_flight_cmds[0]); +#endif + ao_add_task(&flight_task, ao_flight, "flight"); +} diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c index 977cea85..3795eeb3 100644 --- a/src/kernel/ao_report.c +++ b/src/kernel/ao_report.c @@ -168,6 +168,7 @@ ao_report_number(int32_t n) while (i != 0); } +#ifdef HAS_BARO static void ao_report_altitude(void) { @@ -180,6 +181,7 @@ ao_report_altitude(void) } ao_report_number(max_h); } +#endif #if HAS_BATTERY_REPORT static void @@ -276,7 +278,9 @@ ao_report(void) #endif if (ao_report_state == ao_flight_landed) { +#if HAS_BARO ao_report_altitude(); +#endif #if HAS_FLIGHT ao_delay(AO_SEC_TO_TICKS(5)); continue; diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 3f5fc7a9..422ccec6 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -21,6 +21,10 @@ #include #endif +#ifndef HAS_KALMAN +#define HAS_KALMAN 1 +#endif + #if HAS_GYRO #include #endif @@ -57,7 +61,7 @@ angle_t ao_sample_orient; angle_t ao_sample_orients[AO_NUM_ORIENT]; uint8_t ao_sample_orient_pos; #endif -#ifdef HAS_MOTOR_PRESSURE +#if HAS_MOTOR_PRESSURE motor_pressure_t ao_sample_motor_pressure; #endif @@ -340,7 +344,7 @@ ao_sample_preflight_update(void) ++nsamples; else ao_sample_preflight_set(); -#if !HAS_BARO +#if !HAS_BARO && HAS_KALMAN if ((nsamples & 0x3f) == 0) ao_kalman_reset_accumulate(); #endif @@ -406,7 +410,9 @@ ao_sample(void) else { if (ao_flight_state < ao_flight_boost) ao_sample_preflight_update(); +#if HAS_KALMAN ao_kalman(); +#endif #if HAS_GYRO ao_sample_rotate(); #endif -- 2.30.2