X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=5a5d5b72b0adfb887f238d6512c5b91b68b8a016;hb=188f9efadd480de872f86a8eb741e8738db84c6b;hp=24099347ce4466589592c4d49ffc78775de867fe;hpb=24167015705ae831692b95735968b04a876f935e;p=fw%2Faltos diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 24099347..5a5d5b72 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -3,7 +3,8 @@ * * 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. + * 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 @@ -20,7 +21,7 @@ #include #endif -#if HAS_MPU6000 +#if HAS_MPU6000 || HAS_MPU9250 #include #endif @@ -36,33 +37,38 @@ #error Please define HAS_USB #endif +#if HAS_FAKE_FLIGHT +#include +#endif + #ifndef HAS_TELEMETRY #define HAS_TELEMETRY HAS_RADIO #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 */ -__pdata uint16_t ao_motor_number; /* number of motors burned so far */ +enum ao_flight_state ao_flight_state; /* current flight state */ +uint16_t ao_boost_tick; /* time of most recent boost detect */ +uint16_t ao_launch_tick; /* time of first boost detect */ +uint16_t ao_motor_number; /* number of motors burned so far */ #if HAS_SENSOR_ERRORS /* Any sensor can set this to mark the flight computer as 'broken' */ -__xdata uint8_t ao_sensor_errors; +uint8_t ao_sensor_errors; #endif /* * 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 uint16_t ao_interval_end; +static ao_v_t ao_interval_min_height; +static ao_v_t ao_interval_max_height; #if HAS_ACCEL -static __data int16_t ao_coast_avg_accel; +static ao_v_t ao_coast_avg_accel; #endif -__pdata uint8_t ao_flight_force_idle; +uint8_t ao_flight_force_idle; /* We also have a clock, which can be used to sanity check things in * case of other failures @@ -126,11 +132,14 @@ ao_flight(void) { /* Set pad mode - we can fly! */ ao_flight_state = ao_flight_pad; -#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE && !DEBUG /* Disable the USB controller in flight mode * to save power */ - ao_usb_disable(); +#if HAS_FAKE_FLIGHT + if (!ao_fake_flight_active) +#endif + ao_usb_disable(); #endif #if !HAS_ACCEL && PACKET_HAS_SLAVE @@ -166,11 +175,10 @@ ao_flight(void) #endif } /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&ao_flight_state); break; case ao_flight_pad: - /* pad to boost: * * barometer: > 20m vertical motion @@ -192,7 +200,7 @@ ao_flight(void) ) { ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; + ao_launch_tick = ao_boost_tick = ao_sample_tick; /* start logging data */ ao_log_start(); @@ -211,7 +219,7 @@ ao_flight(void) ao_wakeup(&ao_gps_new); #endif - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&ao_flight_state); } break; case ao_flight_boost: @@ -226,7 +234,7 @@ ao_flight(void) * 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)) || + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) || (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) { #if HAS_ACCEL @@ -236,7 +244,7 @@ ao_flight(void) ao_flight_state = ao_flight_coast; #endif ++ao_motor_number; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&ao_flight_state); } break; #if HAS_ACCEL @@ -249,7 +257,7 @@ ao_flight(void) 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)); + ao_wakeup(&ao_flight_state); } else goto check_re_boost; break; @@ -262,7 +270,7 @@ ao_flight(void) * number of seconds. */ if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < + if ((int16_t) (ao_sample_tick - ao_launch_tick) < AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } @@ -275,9 +283,11 @@ ao_flight(void) * the measured altitude reasonably closely; otherwise * we're probably transsonic. */ +#define AO_ERROR_BOUND 100 + if (ao_speed < 0 #if !HAS_ACCEL - && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) + && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < AO_ERROR_BOUND) #endif ) { @@ -296,16 +306,16 @@ ao_flight(void) /* and enter drogue state */ ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&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); + ao_coast_avg_accel = ao_coast_avg_accel + ((ao_accel - ao_coast_avg_accel) >> 5); 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)); + ao_wakeup(&ao_flight_state); } } #endif @@ -342,7 +352,7 @@ ao_flight(void) 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)); + ao_wakeup(&ao_flight_state); } break; @@ -364,10 +374,12 @@ ao_flight(void) { ao_flight_state = ao_flight_landed; +#if HAS_ADC /* turn off the ADC capture */ ao_timer_set_adc_interval(0); +#endif - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&ao_flight_state); } ao_interval_min_height = ao_interval_max_height = ao_avg_height; ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; @@ -392,14 +404,14 @@ ao_flight(void) } #if HAS_FLIGHT_DEBUG -static inline int int_part(int16_t i) { return i >> 4; } -static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } +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 - int16_t accel; + ao_v_t accel; accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; #endif @@ -432,7 +444,9 @@ ao_flight_dump(void) printf (" max_height %d\n", ao_max_height); printf (" avg_height %d\n", ao_avg_height); printf (" error_h %d\n", ao_error_h); +#if !HAS_ACCEL printf (" error_avg %d\n", ao_error_h_sq_avg); +#endif } static void @@ -451,7 +465,7 @@ ao_orient_test_select(void) ao_orient_test = !ao_orient_test; } -__code struct ao_cmds ao_flight_cmds[] = { +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" }, @@ -459,7 +473,7 @@ __code struct ao_cmds ao_flight_cmds[] = { }; #endif -static __xdata struct ao_task flight_task; +static struct ao_task flight_task; void ao_flight_init(void)