X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=c5069158a61cc603914566a4917608a54a300be3;hp=c2700d20bddb41801bacb0d560291a50aaad4bb0;hb=d779d8e5b1106aaec6170761e6f5bd4e8d5ac6e7;hpb=0d57c78dde3c6e61576a4769b0e0fae7e88c107d diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index c2700d20..c5069158 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -21,6 +21,8 @@ #include #endif +#include + #if HAS_MPU6000 || HAS_MPU9250 #include #endif @@ -47,28 +49,45 @@ /* Main flight thread. */ -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_boost_tick; /* time of most recent boost detect */ -__pdata uint16_t ao_launch_tick; /* time of first boost 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 ao_v_t ao_interval_min_height; -static __data ao_v_t ao_interval_max_height; +static uint16_t ao_interval_end; +#ifdef HAS_BARO +static ao_v_t ao_interval_min_height; +static ao_v_t ao_interval_max_height; +#else +static accel_t ao_interval_min_accel_along, ao_interval_max_accel_along; +static accel_t ao_interval_min_accel_across, ao_interval_max_accel_across; +static accel_t ao_interval_min_accel_through, ao_interval_max_accel_through; +#endif #if HAS_ACCEL -static __data ao_v_t ao_coast_avg_accel; +static ao_v_t ao_coast_avg_accel; #endif -__pdata uint8_t ao_flight_force_idle; +#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; /* We also have a clock, which can be used to sanity check things in * case of other failures @@ -109,9 +128,12 @@ ao_flight(void) 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 || - ao_ground_height < -1000 || - ao_ground_height > 7000) + ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP +#if HAS_BARO + || ao_ground_height < -1000 || + ao_ground_height > 7000 +#endif + ) { /* Detected an accel value outside -1.5g to 1.5g * (or uncalibrated values), so we go into invalid mode @@ -175,7 +197,7 @@ 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: @@ -203,7 +225,9 @@ ao_flight(void) ao_launch_tick = ao_boost_tick = ao_sample_tick; /* start logging data */ +#if HAS_LOG ao_log_start(); +#endif #if HAS_TELEMETRY /* Increase telemetry rate */ @@ -219,7 +243,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: @@ -238,16 +262,27 @@ ao_flight(void) (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) { #if HAS_ACCEL +#if HAS_BARO ao_flight_state = ao_flight_fast; +#else + ao_flight_state = ao_flight_coast; + + /* Initialize landing detection interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + init_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along); + init_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across); + init_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through); +#endif ao_coast_avg_accel = ao_accel; #else 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 +#if HAS_ACCEL && HAS_BARO case ao_flight_fast: /* * This is essentially the same as coast, @@ -257,13 +292,14 @@ 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; #endif case ao_flight_coast: +#if HAS_BARO /* * By customer request - allow the user * to lock out apogee detection for a specified @@ -306,21 +342,58 @@ 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); + } + else +#else /* not HAS_BARO */ + /* coast to land: + * + * accel: values stable + */ + check_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along); + check_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across); + check_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through); + +#define MAX_QUIET_ACCEL 2 + + if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_accel_along - ao_interval_min_accel_along <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) && + ao_interval_max_accel_across - ao_interval_min_accel_across <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) && + ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL)) + { + ao_flight_state = ao_flight_landed; +#if HAS_ADC + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); +#endif + + ao_wakeup(&ao_flight_state); + } + + /* Reset interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + init_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along); + init_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across); + init_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through); } +#endif #if HAS_ACCEL - else { + { +#if HAS_BARO check_re_boost: +#endif 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 break; +#if HAS_BARO case ao_flight_drogue: /* drogue to main deploy: @@ -352,7 +425,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; @@ -363,7 +436,6 @@ ao_flight(void) * * 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) @@ -379,12 +451,13 @@ ao_flight(void) 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; } break; +#endif /* HAS_BARO */ #if HAS_FLIGHT_DEBUG case ao_flight_test: #if HAS_GYRO @@ -418,20 +491,27 @@ ao_flight_dump(void) printf ("sample:\n"); printf (" tick %d\n", ao_sample_tick); +#if HAS_BARO printf (" raw pres %d\n", ao_sample_pres); +#endif #if HAS_ACCEL printf (" raw accel %d\n", ao_sample_accel); #endif +#if HAS_BARO printf (" ground pres %d\n", ao_ground_pres); printf (" ground alt %d\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 %d\n", ao_sample_alt); printf (" height %d\n", ao_sample_height); +#endif + #if HAS_ACCEL printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); #endif @@ -444,7 +524,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 @@ -463,7 +545,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" }, @@ -471,7 +553,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)