From: Keith Packard Date: Tue, 10 Jul 2012 22:07:34 +0000 (-0700) Subject: altos: remove stale ao_flight.c and ao_sample.c X-Git-Tag: 1.1~119 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=702ca87983594880d7926d2317d63802af82746e altos: remove stale ao_flight.c and ao_sample.c The mega-metrum versions are now the official versions Signed-off-by: Keith Packard --- diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c deleted file mode 100644 index 494e656d..00000000 --- a/src/core/ao_flight.c +++ /dev/null @@ -1,350 +0,0 @@ -/* - * Copyright © 2009 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; version 2 of the License. - * - * 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" -#endif - -#ifndef HAS_ACCEL -#error Please define HAS_ACCEL -#endif - -#ifndef HAS_GPS -#error Please define HAS_GPS -#endif - -#ifndef HAS_USB -#error Please define HAS_USB -#endif - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_boost_tick; /* time of launch detect */ - -/* - * track min/max data over a long interval to detect - * resting - */ -static __data uint16_t ao_interval_end; -static __data int16_t ao_interval_min_height; -static __data int16_t ao_interval_max_height; -static __data int16_t ao_coast_avg_accel; - -__pdata uint8_t ao_flight_force_idle; - -/* We also have a clock, which can be used to sanity check things in - * case of other failures - */ - -#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) - -/* Landing is detected by getting constant readings from both pressure and accelerometer - * for a fairly long time (AO_INTERVAL_TICKS) - */ -#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) - -#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: - - /* Check to see what mode we should go to. - * - Invalid mode if accel cal appears to be out - * - pad mode if we're upright, - * - idle mode otherwise - */ -#if HAS_ACCEL - if (ao_config.accel_plus_g == 0 || - ao_config.accel_minus_g == 0 || - ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || - ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP) - { - /* Detected an accel value outside -1.5g to 1.5g - * (or uncalibrated values), so we go into invalid mode - */ - ao_flight_state = ao_flight_invalid; - - /* Turn on packet system in invalid mode on TeleMetrum */ - ao_packet_slave_start(); - } else -#endif - if (!ao_flight_force_idle -#if HAS_ACCEL - && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP -#endif - ) - { - /* Set pad mode - we can fly! */ - ao_flight_state = ao_flight_pad; -#if HAS_USB - /* Disable the USB controller in flight mode - * to save power - */ - ao_usb_disable(); -#endif - -#if !HAS_ACCEL - /* Disable packet mode in pad state on TeleMini */ - ao_packet_slave_stop(); -#endif - - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } else { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; - -#if HAS_ACCEL - /* Turn on packet system in idle mode on TeleMetrum */ - ao_packet_slave_start(); -#endif - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - - break; - case ao_flight_pad: - - /* pad to boost: - * - * barometer: > 20m vertical motion - * OR - * accelerometer: > 2g AND velocity > 5m/s - * - * The accelerometer should always detect motion before - * the barometer, but we use both to make sure this - * transition is detected. If the device - * doesn't have an accelerometer, then ignore the - * speed and acceleration as they are quite noisy - * on the pad. - */ - if (ao_height > AO_M_TO_HEIGHT(20) -#if HAS_ACCEL - || (ao_accel > AO_MSS_TO_ACCEL(20) && - ao_speed > AO_MS_TO_SPEED(5)) -#endif - ) - { - ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; - - /* start logging data */ - ao_log_start(); - - /* Increase telemetry rate */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); - - /* disable RDF beacon */ - ao_rdf_set(0); - -#if HAS_GPS - /* Record current GPS position by waking up GPS log tasks */ - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); -#endif - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - case ao_flight_boost: - - /* boost to fast: - * - * accelerometer: start to fall at > 1/4 G - * OR - * time: boost for more than 15 seconds - * - * Detects motor burn out by the switch from acceleration to - * deceleration, or by waiting until the maximum burn duration - * (15 seconds) has past. - */ - if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || - (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) - { -#if HAS_ACCEL - ao_flight_state = ao_flight_fast; - ao_coast_avg_accel = ao_accel; -#else - ao_flight_state = ao_flight_coast; -#endif - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; -#if HAS_ACCEL - case ao_flight_fast: - /* - * This is essentially the same as coast, - * but the barometer is being ignored as - * it may be unreliable. - */ - if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) - { - ao_flight_state = ao_flight_coast; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } else - goto check_re_boost; - break; -#endif - case ao_flight_coast: - - /* - * By customer request - allow the user - * to lock out apogee detection for a specified - * number of seconds. - */ - if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < - AO_SEC_TO_TICKS(ao_config.apogee_lockout)) - break; - } - - /* apogee detect: coast to drogue deploy: - * - * speed: < 0 - * - * Also make sure the model altitude is tracking - * the measured altitude reasonably closely; otherwise - * we're probably transsonic. - */ - if (ao_speed < 0 -#if !HAS_ACCEL - && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) -#endif - ) - { - /* ignite the drogue charge */ - ao_ignite(ao_igniter_drogue); - - /* slow down the telemetry system */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); - - /* Turn the RDF beacon back on */ - ao_rdf_set(1); - - /* and enter drogue state */ - ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } -#if HAS_ACCEL - else { - check_re_boost: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); - if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { - ao_boost_tick = ao_sample_tick; - ao_flight_state = ao_flight_boost; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - } -#endif - - break; - case ao_flight_drogue: - - /* drogue to main deploy: - * - * barometer: reach main deploy altitude - * - * Would like to use the accelerometer for this test, but - * the orientation of the flight computer is unknown after - * drogue deploy, so we ignore it. Could also detect - * high descent rate using the pressure sensor to - * recognize drogue deploy failure and eject the main - * at that point. Perhaps also use the drogue sense lines - * to notice continutity? - */ - if (ao_height <= ao_config.main_deploy) - { - ao_ignite(ao_igniter_main); - - /* - * Start recording min/max height - * to figure out when the rocket has landed - */ - - /* initialize interval values */ - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - - ao_flight_state = ao_flight_main; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - - /* fall through... */ - case ao_flight_main: - - /* main to land: - * - * barometer: altitude stable - */ - - if (ao_avg_height < ao_interval_min_height) - ao_interval_min_height = ao_avg_height; - if (ao_avg_height > ao_interval_max_height) - ao_interval_max_height = ao_avg_height; - - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { - if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) - { - ao_flight_state = ao_flight_landed; - - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - } - break; - case ao_flight_landed: - break; - } - } -} - -static __xdata struct ao_task flight_task; - -void -ao_flight_init(void) -{ - ao_flight_state = ao_flight_startup; - ao_add_task(&flight_task, ao_flight, "flight"); -} diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c deleted file mode 100644 index 2bf9632f..00000000 --- a/src/core/ao_sample.c +++ /dev/null @@ -1,214 +0,0 @@ -/* - * Copyright © 2011 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; version 2 of the License. - * - * 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" -#endif - -/* - * Current sensor values - */ - -__pdata uint16_t ao_sample_tick; /* time of last data */ -__pdata int16_t ao_sample_pres; -__pdata int16_t ao_sample_alt; -__pdata int16_t ao_sample_height; -#if HAS_ACCEL -__pdata int16_t ao_sample_accel; -#endif - -__data uint8_t ao_sample_adc; - -/* - * Sensor calibration values - */ - -__pdata int16_t ao_ground_pres; /* startup pressure */ -__pdata int16_t ao_ground_height; /* MSL of ao_ground_pres */ - -#if HAS_ACCEL -__pdata int16_t ao_ground_accel; /* startup acceleration */ -__pdata int16_t ao_accel_2g; /* factory accel calibration */ -__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ -#endif - -static __pdata uint8_t ao_preflight; /* in preflight mode */ - -static __pdata uint16_t nsamples; -__pdata int32_t ao_sample_pres_sum; -#if HAS_ACCEL -__pdata int32_t ao_sample_accel_sum; -#endif - -static void -ao_sample_preflight(void) -{ - /* startup state: - * - * Collect 512 samples of acceleration and pressure - * data and average them to find the resting values - */ - if (nsamples < 512) { -#if HAS_ACCEL - ao_sample_accel_sum += ao_sample_accel; -#endif - ao_sample_pres_sum += ao_sample_pres; - ++nsamples; - } else { - ao_config_get(); -#if HAS_ACCEL - ao_ground_accel = ao_sample_accel_sum >> 9; - ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; - ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; -#endif - ao_ground_pres = ao_sample_pres_sum >> 9; - ao_ground_height = ao_pres_to_altitude(ao_ground_pres); - ao_preflight = FALSE; - } -} - -uint8_t -ao_sample(void) -{ - ao_wakeup(DATA_TO_XDATA(&ao_sample_adc)); - ao_sleep(DATA_TO_XDATA(&ao_adc_head)); - while (ao_sample_adc != ao_adc_head) { - __xdata struct ao_adc *ao_adc; - - /* Capture a sample */ - ao_adc = &ao_adc_ring[ao_sample_adc]; - ao_sample_tick = ao_adc->tick; - ao_sample_pres = ao_adc->pres; - ao_sample_alt = ao_pres_to_altitude(ao_sample_pres); - ao_sample_height = ao_sample_alt - ao_ground_height; -#if HAS_ACCEL - ao_sample_accel = ao_adc->accel; -#if HAS_ACCEL_REF - /* - * Ok, the math here is a bit tricky. - * - * ao_sample_accel: ADC output for acceleration - * ao_accel_ref: ADC output for the 5V reference. - * ao_cook_accel: Corrected acceleration value - * Vcc: 3.3V supply to the CC1111 - * Vac: 5V supply to the accelerometer - * accel: input voltage to accelerometer ADC pin - * ref: input voltage to 5V reference ADC pin - * - * - * Measured acceleration is ratiometric to Vcc: - * - * ao_sample_accel accel - * ------------ = ----- - * 32767 Vcc - * - * Measured 5v reference is also ratiometric to Vcc: - * - * ao_accel_ref ref - * ------------ = ----- - * 32767 Vcc - * - * - * ao_accel_ref = 32767 * (ref / Vcc) - * - * Acceleration is measured ratiometric to the 5V supply, - * so what we want is: - * - * ao_cook_accel accel - * ------------- = ----- - * 32767 ref - * - * - * accel Vcc - * = ----- * --- - * Vcc ref - * - * ao_sample_accel 32767 - * = ------------ * ------------ - * 32767 ao_accel_ref - * - * Multiply through by 32767: - * - * ao_sample_accel * 32767 - * ao_cook_accel = -------------------- - * ao_accel_ref - * - * Now, the tricky part. Getting this to compile efficiently - * and keeping all of the values in-range. - * - * First off, we need to use a shift of 16 instead of * 32767 as SDCC - * does the obvious optimizations for byte-granularity shifts: - * - * ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref - * - * Next, lets check our input ranges: - * - * 0 <= ao_sample_accel <= 0x7fff (singled ended ADC conversion) - * 0x7000 <= ao_accel_ref <= 0x7fff (the 5V ref value is close to 0x7fff) - * - * Plugging in our input ranges, we get an output range of 0 - 0x12490, - * which is 17 bits. That won't work. If we take the accel ref and shift - * by a bit, we'll change its range: - * - * 0xe000 <= ao_accel_ref<<1 <= 0xfffe - * - * ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1) - * - * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It - * is, however, one bit too large for our signed computations. So, we - * take the result and shift that by a bit: - * - * ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1 - * - * This finally creates an output range of 0 - 0x4924. As the ADC only - * provides 11 bits of data, we haven't actually lost any precision, - * just dropped a bit of noise off the low end. - */ - ao_sample_accel = (uint16_t) ((((uint32_t) ao_sample_accel << 16) / (ao_accel_ref[ao_sample_adc] << 1))) >> 1; - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - ao_sample_accel = 0x7fff - ao_sample_accel; - ao_adc->accel = ao_sample_accel; -#else - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) { - ao_sample_accel = 0x7fff - ao_sample_accel; - ao_adc->accel = ao_sample_accel; - } -#endif -#endif - - if (ao_preflight) - ao_sample_preflight(); - else - ao_kalman(); - ao_sample_adc = ao_adc_ring_next(ao_sample_adc); - } - return !ao_preflight; -} - -void -ao_sample_init(void) -{ - nsamples = 0; - ao_sample_pres_sum = 0; - ao_sample_pres = 0; -#if HAS_ACCEL - ao_sample_accel_sum = 0; - ao_sample_accel = 0; -#endif - ao_sample_adc = ao_adc_head; - ao_preflight = TRUE; -}