X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=8553ccc5854f8f4d27569e1ae8928aaf7fef63da;hp=50f2b68f998f3df1c7960bfb64ffb6153e15de6f;hb=7f46240dfc57164f0c1b0c4c4ed9695bca63860d;hpb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 50f2b68f..8553ccc5 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -21,7 +21,9 @@ #include #endif -#if HAS_MPU6000 +#include + +#if HAS_MPU6000 || HAS_MPU9250 #include #endif @@ -47,27 +49,45 @@ /* 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 */ +AO_TICK_TYPE ao_boost_tick; /* time of most recent boost detect */ +AO_TICK_TYPE 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 @@ -82,6 +102,10 @@ __pdata uint8_t ao_flight_force_idle; #define abs(a) ((a) < 0 ? -(a) : (a)) +#if !HAS_BARO +// #define DEBUG_ACCEL_ONLY 1 +#endif + void ao_flight(void) { @@ -107,10 +131,13 @@ ao_flight(void) #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 || - ao_ground_height < -1000 || - ao_ground_height > 7000) + ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP || + ao_ground_accel > (accel_t) 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 @@ -174,9 +201,18 @@ ao_flight(void) #endif } /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(&ao_flight_state); + + break; +#if DEBUG_ACCEL_ONLY + case ao_flight_invalid: + case ao_flight_idle: + printf("+g %d ga %d sa %d accel %ld speed %ld\n", + ao_config.accel_plus_g, ao_ground_accel, ao_sample_accel, ao_accel, ao_speed); break; +#endif + case ao_flight_pad: /* pad to boost: * @@ -193,16 +229,20 @@ ao_flight(void) */ 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)) + || (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; + ao_wakeup(&ao_flight_state); + + 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 */ @@ -217,8 +257,6 @@ ao_flight(void) ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; ao_wakeup(&ao_gps_new); #endif - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } break; case ao_flight_boost: @@ -233,20 +271,31 @@ 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)) || - (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) || + (AO_TICK_SIGNED) (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_wakeup(&ao_flight_state); ++ao_motor_number; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } break; -#if HAS_ACCEL +#if HAS_ACCEL && HAS_BARO case ao_flight_fast: /* * This is essentially the same as coast, @@ -256,20 +305,21 @@ 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 * number of seconds. */ if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_launch_tick) < AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } @@ -282,17 +332,17 @@ 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 ) { -#if HAS_IGNITE - /* ignite the drogue charge */ - ao_ignite(ao_igniter_drogue); -#endif - + /* enter drogue state */ + ao_flight_state = ao_flight_drogue; + ao_wakeup(&ao_flight_state); #if HAS_TELEMETRY /* slow down the telemetry system */ ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); @@ -300,24 +350,56 @@ ao_flight(void) /* Turn the RDF beacon back on */ ao_rdf_set(1); #endif + } + 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 ((AO_TICK_SIGNED) (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; + ao_wakeup(&ao_flight_state); +#if HAS_ADC + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); +#endif + } - /* and enter drogue state */ - ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&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: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); +#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: @@ -334,9 +416,8 @@ ao_flight(void) */ if (ao_height <= ao_config.main_deploy) { -#if HAS_IGNITE - ao_ignite(ao_igniter_main); -#endif + ao_flight_state = ao_flight_main; + ao_wakeup(&ao_flight_state); /* * Start recording min/max height @@ -347,9 +428,6 @@ ao_flight(void) 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; @@ -360,32 +438,30 @@ 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) ao_interval_max_height = ao_avg_height; - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if ((AO_TICK_SIGNED) (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; - + ao_wakeup(&ao_flight_state); #if HAS_ADC /* turn off the ADC capture */ ao_timer_set_adc_interval(0); #endif - - 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; +#endif /* HAS_BARO */ #if HAS_FLIGHT_DEBUG case ao_flight_test: #if HAS_GYRO - printf ("angle %4d pitch %7d yaw %7d roll %7d\n", + printf ("angle %4d pitch %7ld yaw %7ld roll %7ld\n", ao_sample_orient, ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, @@ -415,33 +491,42 @@ ao_flight_dump(void) printf ("sample:\n"); printf (" tick %d\n", ao_sample_tick); - printf (" raw pres %d\n", ao_sample_pres); +#if HAS_BARO + printf (" raw pres %ld\n", ao_sample_pres); +#endif #if HAS_ACCEL printf (" raw accel %d\n", ao_sample_accel); #endif - printf (" ground pres %d\n", ao_ground_pres); - printf (" ground alt %d\n", ao_ground_height); +#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 - printf (" alt %d\n", ao_sample_alt); - printf (" height %d\n", ao_sample_height); +#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 %d\n", ao_height); + 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 %d\n", ao_max_height); - printf (" avg_height %d\n", ao_avg_height); - printf (" error_h %d\n", ao_error_h); + 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 @@ -458,9 +543,10 @@ static void ao_orient_test_select(void) { ao_orient_test = !ao_orient_test; + printf("orient test %d\n", 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" }, @@ -468,7 +554,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)