X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_flight.c;h=e0fd97f2464b901a61649037e61a9848e46b819e;hp=c0f5683047b4d5e3e37dd6d28d14bb9de71295e2;hb=0c2533be15858774ef9381aa8c8344356fd5b971;hpb=210dbaa23cdacf3a6f2d6e23493e96ee2ac9bca7 diff --git a/src/ao_flight.c b/src/ao_flight.c index c0f56830..e0fd97f2 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -48,6 +48,9 @@ __pdata int16_t ao_interval_max_pres; __data uint8_t ao_flight_adc; __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres; +__pdata int16_t ao_accel_2g; + +__xdata uint8_t ao_flight_force_idle; /* Accelerometer calibration * @@ -72,19 +75,18 @@ __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres; #define GRAVITY 9.80665 /* convert m/s to velocity count */ -#define VEL_MPS_TO_COUNT(mps) ((int32_t) (((mps) / GRAVITY) * ACCEL_G * 100)) - -#define ACCEL_G 265 -#define ACCEL_ZERO_G 16000 -#define ACCEL_NOSE_UP (ACCEL_G * 2 /3) -#define ACCEL_BOOST ACCEL_G * 2 -#define ACCEL_INT_LAND (ACCEL_G / 10) -#define ACCEL_VEL_LAND VEL_MPS_TO_COUNT(10) +#define VEL_MPS_TO_COUNT(mps) (((int32_t) (((mps) / GRAVITY) * (AO_HERTZ/2))) * (int32_t) ao_accel_2g) + +#define ACCEL_NOSE_UP (ao_accel_2g >> 2) +#define ACCEL_BOOST ao_accel_2g +#define ACCEL_COAST (ao_accel_2g >> 3) +#define ACCEL_INT_LAND (ao_accel_2g >> 3) #define ACCEL_VEL_MACH VEL_MPS_TO_COUNT(200) -#define ACCEL_VEL_APOGEE VEL_MPS_TO_COUNT(2) -#define ACCEL_VEL_MAIN VEL_MPS_TO_COUNT(100) #define ACCEL_VEL_BOOST VEL_MPS_TO_COUNT(5) +int32_t accel_vel_mach; +int32_t accel_vel_boost; + /* * Barometer calibration * @@ -132,7 +134,7 @@ __xdata int32_t ao_raw_accel_sum, ao_raw_pres_sum; /* 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(20) +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) #define abs(a) ((a) < 0 ? -(a) : (a)) @@ -170,14 +172,14 @@ ao_flight(void) * so subtract instead of add. */ ticks = ao_flight_tick - ao_flight_prev_tick; - ao_vel_change = (((ao_raw_accel >> 1) + (ao_raw_accel_prev >> 1)) - ao_ground_accel); + ao_vel_change = ao_ground_accel - (((ao_raw_accel + 1) >> 1) + ((ao_raw_accel_prev + 1) >> 1)); ao_raw_accel_prev = ao_raw_accel; /* one is a common interval */ if (ticks == 1) - ao_flight_vel -= (int32_t) ao_vel_change; + ao_flight_vel += (int32_t) ao_vel_change; else - ao_flight_vel -= (int32_t) ao_vel_change * (int32_t) ticks; + ao_flight_vel += (int32_t) ao_vel_change * (int32_t) ticks; ao_flight_adc = ao_adc_ring_next(ao_flight_adc); } @@ -211,15 +213,21 @@ ao_flight(void) ao_min_pres = ao_ground_pres; ao_config_get(); ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy); + ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; + accel_vel_mach = ACCEL_VEL_MACH; + accel_vel_boost = ACCEL_VEL_BOOST; ao_flight_vel = 0; ao_min_vel = 0; ao_old_vel = ao_flight_vel; ao_old_vel_tick = ao_flight_tick; - /* Go to launchpad state if the nose is pointing up */ + /* Go to pad state if the nose is pointing up */ ao_config_get(); - if (ao_flight_accel < ao_config.accel_zero_g - ACCEL_NOSE_UP) { - + if (ao_config.accel_plus_g != 0 && + ao_config.accel_minus_g != 0 && + ao_flight_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP && + !ao_flight_force_idle) + { /* Disable the USB controller in flight mode * to save power */ @@ -227,22 +235,23 @@ ao_flight(void) /* Turn on telemetry system */ + ao_rdf_set(1); ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); - ao_flight_state = ao_flight_launchpad; + ao_flight_state = ao_flight_pad; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } else { ao_flight_state = ao_flight_idle; - /* Turn on the Green LED in idle mode + /* Turn on packet system in idle mode */ - ao_led_on(AO_LED_GREEN); + ao_packet_slave_start(); ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } /* signal successful initialization by turning off the LED */ ao_led_off(AO_LED_RED); break; - case ao_flight_launchpad: + case ao_flight_pad: /* Trim velocity * @@ -277,13 +286,16 @@ ao_flight(void) /* Increase telemetry rate */ ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); + /* disable RDF beacon */ + ao_rdf_set(0); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; case ao_flight_boost: - /* boost to coast: + /* boost to fast: * * accelerometer: start to fall at > 1/4 G * OR @@ -293,17 +305,17 @@ ao_flight(void) * deceleration, or by waiting until the maximum burn duration * (15 seconds) has past. */ - if (ao_flight_accel > ao_ground_accel + (ACCEL_G >> 2) || + if (ao_flight_accel > ao_ground_accel + ACCEL_COAST || (int16_t) (ao_flight_tick - ao_launch_tick) > BOOST_TICKS_MAX) { - ao_flight_state = ao_flight_coast; + ao_flight_state = ao_flight_fast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; - case ao_flight_coast: + case ao_flight_fast: - /* coast to apogee detect: + /* fast to coast: * * accelerometer: integrated velocity < 200 m/s * OR @@ -325,27 +337,23 @@ ao_flight(void) * apogee detect */ ao_min_vel = abs(ao_flight_vel); - ao_flight_state = ao_flight_apogee; + ao_flight_state = ao_flight_coast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } break; - case ao_flight_apogee: + case ao_flight_coast: - /* apogee detect to drogue deploy: + /* apogee detect: coast to drogue deploy: * - * accelerometer: abs(velocity) > min_velocity + 2m/s - * OR * barometer: fall at least 10m * - * If the barometer saturates because the flight - * goes over its measuring range (about 53k'), - * requiring a 10m fall will avoid prematurely - * detecting apogee; the accelerometer will take - * over in that case and the integrated velocity - * measurement should suffice to find apogee + * It would be nice to use the accelerometer + * to detect apogee as well, but tests have + * shown that flights far from vertical would + * grossly mis-detect apogee. So, for now, + * we'll trust to a single sensor for this test */ - if (/* abs(ao_flight_vel) > ao_min_vel + ACCEL_VEL_APOGEE || */ - ao_flight_pres > ao_min_pres + BARO_APOGEE) + if (ao_flight_pres > ao_min_pres + BARO_APOGEE) { /* ignite the drogue charge */ ao_ignite(ao_igniter_drogue); @@ -428,18 +436,20 @@ ao_flight(void) ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS; ao_interval_cur_min_pres = ao_interval_cur_max_pres = ao_flight_pres; ao_interval_cur_min_accel = ao_interval_cur_max_accel = ao_flight_accel; - } - if ((uint16_t) (ao_interval_max_accel - ao_interval_min_accel) < (uint16_t) ACCEL_INT_LAND && - ao_flight_pres > ao_ground_pres - BARO_LAND && - (uint16_t) (ao_interval_max_pres - ao_interval_min_pres) < (uint16_t) BARO_INT_LAND) - { - ao_flight_state = ao_flight_landed; + if ((uint16_t) (ao_interval_max_accel - ao_interval_min_accel) < (uint16_t) ACCEL_INT_LAND && + ao_flight_pres > ao_ground_pres - BARO_LAND && + (uint16_t) (ao_interval_max_pres - ao_interval_min_pres) < (uint16_t) BARO_INT_LAND) + { + ao_flight_state = ao_flight_landed; - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); + /* Enable RDF beacon */ + ao_rdf_set(1); - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } } break; case ao_flight_landed: @@ -448,27 +458,8 @@ ao_flight(void) } } -#define AO_ACCEL_COUNT_TO_MSS(count) ((count) / 27) -#define AO_VEL_COUNT_TO_MS(count) ((int16_t) ((count) / 2700)) - -static void -ao_flight_status(void) -{ - printf("STATE: %7s accel: %d speed: %d altitude: %d main: %d\n", - ao_state_names[ao_flight_state], - AO_ACCEL_COUNT_TO_MSS(ACCEL_ZERO_G - ao_flight_accel), - AO_VEL_COUNT_TO_MS(ao_flight_vel), - ao_pres_to_altitude(ao_flight_pres), - ao_pres_to_altitude(ao_main_pres)); -} - static __xdata struct ao_task flight_task; -__code struct ao_cmds ao_flight_cmds[] = { - { 'f', ao_flight_status, "f Display current flight state" }, - { 0, ao_flight_status, NULL } -}; - void ao_flight_init(void) { @@ -480,5 +471,4 @@ ao_flight_init(void) ao_interval_end = AO_INTERVAL_TICKS; ao_add_task(&flight_task, ao_flight, "flight"); - ao_cmd_register(&ao_flight_cmds[0]); }